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         UHCI_int_AppendTD(Cont, qh, TD);
 425 }
 426
 427 void *UHCI_InterruptIN(void *Ptr, int Dest, int Period, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 428 {
 429         tUHCI_TD        *td;
 430
 431         if( Period < 0 )        return NULL;
 432
 433         ENTER("pPtr xDest iPeriod pCb pCbData pBuf, iLength",
 434                 Ptr, Dest, Period, Cb, CbData, Buf, Length);
 435
 436         // TODO: Data toggle?
 437         td = UHCI_int_CreateTD(Ptr, Dest, PID_IN, 0, Cb, CbData, Buf, Length);
 438         if( !td )       return NULL;
 439
 440         UHCI_int_SetInterruptPoll(Ptr, td, Period);
 441
 442         LEAVE('p', td);
 443         return td;
 444 }
 445 // TODO: Does interrupt OUT make sense?
 446 void *UHCI_InterruptOUT(void *Ptr, int Dest, int Period, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 447 {
 448         tUHCI_TD        *td;
 449
 450         if( Period < 0 )        return NULL;
 451
 452         ENTER("pPtr xDest iPeriod pCb pCbData pBuf, iLength",
 453                 Ptr, Dest, Period, Cb, CbData, Buf, Length);
 454
 455         // TODO: Data toggle?
 456         td = UHCI_int_CreateTD(Ptr, Dest, PID_OUT, 0, Cb, CbData, Buf, Length);
 457         if( !td )       return NULL;
 458
 459         UHCI_int_SetInterruptPoll(Ptr, td, Period);
 460
 461         LEAVE('p', td);
 462         return td;
 463 }
 464
 465 void UHCI_StopInterrupt(void *Ptr, void *Handle)
 466 {
 467         // TODO: Stop interrupt transaction
 468 }
 469
 470 void *UHCI_ControlSETUP(void *Ptr, int Dest, int Tgl, void *Data, size_t Length)
 471 {
 472         tUHCI_Controller        *Cont = Ptr;
 473         tUHCI_QH        *qh = &Cont->TDQHPage->ControlQH;
 474         tUHCI_TD        *td;
 475
 476         ENTER("pPtr xDest iTgl pData iLength", Ptr, Dest, Tgl, Data, Length);
 477
 478         td = UHCI_int_CreateTD(Cont, Dest, PID_SETUP, Tgl, NULL, NULL, Data, Length);
 479         UHCI_int_AppendTD(Cont, qh, td);
 480
 481         LEAVE('p', td);
 482
 483         return td;
 484 }
 485 void *UHCI_ControlOUT(void *Ptr, int Dest, int Tgl, tUSBHostCb Cb, void *CbData, void *Data, size_t Length)
 486 {
 487         tUHCI_Controller        *Cont = Ptr;
 488         tUHCI_QH        *qh = &Cont->TDQHPage->ControlQH;
 489         tUHCI_TD        *td;
 490
 491         ENTER("pPtr xDest iTgl pCb pCbData pData iLength", Ptr, Dest, Tgl, Cb, CbData, Data, Length);
 492
 493         td = UHCI_int_CreateTD(Cont, Dest, PID_OUT, Tgl, Cb, CbData, Data, Length);
 494         UHCI_int_AppendTD(Cont, qh, td);
 495
 496         LEAVE('p', td);
 497         return td;
 498 }
 499 void *UHCI_ControlIN(void *Ptr, int Dest, int Tgl, tUSBHostCb Cb, void *CbData, void *Data, size_t Length)
 500 {
 501         tUHCI_Controller        *Cont = Ptr;
 502         tUHCI_QH        *qh = &Cont->TDQHPage->ControlQH;
 503         tUHCI_TD        *td;
 504
 505         ENTER("pPtr xDest iTgl pCb pCbData pData iLength", Ptr, Dest, Tgl, Cb, CbData, Data, Length);
 506
 507         td = UHCI_int_CreateTD(Cont, Dest, PID_IN, !!Tgl, Cb, CbData, Data, Length);
 508         UHCI_int_AppendTD(Cont, qh, td);
 509
 510         LEAVE('p', td);
 511         return td;
 512 }
 513
 514 void *UHCI_BulkOUT(void *Ptr, int Dest, int bToggle, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 515 {
 516         tUHCI_Controller        *Cont = Ptr;
 517         tUHCI_QH        *qh = &Cont->TDQHPage->BulkQH;
 518         tUHCI_TD        *td;
 519         char    *src = Buf;
 520
 521         ENTER("pPtr xDest ibToggle pCb pCbData pData iLength", Ptr, Dest, bToggle, Cb, CbData, Buf, Length);
 522
 523         while( Length > MAX_PACKET_SIZE )
 524         {
 525                 LOG("MaxPacket (rem = %i)", Length);
 526                 td = UHCI_int_CreateTD(Cont, Dest, PID_OUT, bToggle, NULL, NULL, src, MAX_PACKET_SIZE);
 527                 UHCI_int_AppendTD(Cont, qh, td);
 528
 529                 bToggle = !bToggle;
 530                 Length -= MAX_PACKET_SIZE;
 531                 src += MAX_PACKET_SIZE;
 532         }
 533
 534         LOG("Final");
 535         td = UHCI_int_CreateTD(Cont, Dest, PID_OUT, bToggle, NULL, NULL, src, Length);
 536         UHCI_int_AppendTD(Cont, qh, td);
 537
 538         LEAVE('p', td);
 539         return td;
 540 }
 541 void *UHCI_BulkIN(void *Ptr, int Dest, int bToggle, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 542 {
 543         tUHCI_Controller        *Cont = Ptr;
 544         tUHCI_QH        *qh = &Cont->TDQHPage->BulkQH;
 545         tUHCI_TD        *td;
 546         char    *dst = Buf;
 547
 548         ENTER("pPtr xDest ibToggle pCb pCbData pData iLength", Ptr, Dest, bToggle, Cb, CbData, Buf, Length);
 549         while( Length > MAX_PACKET_SIZE )
 550         {
 551                 LOG("MaxPacket (rem = %i)", Length);
 552                 td = UHCI_int_CreateTD(Cont, Dest, PID_IN, bToggle, NULL, NULL, dst, MAX_PACKET_SIZE);
 553                 UHCI_int_AppendTD(Cont, qh, td);
 554
 555                 bToggle = !bToggle;
 556                 Length -= MAX_PACKET_SIZE;
 557                 dst += MAX_PACKET_SIZE;
 558         }
 559
 560         LOG("Final");
 561         td = UHCI_int_CreateTD(Cont, Dest, PID_IN, bToggle, NULL, NULL, dst, Length);
 562         UHCI_int_AppendTD(Cont, qh, td);
 563
 564         LEAVE('p', td);
 565         return td;
 566 }
 567
 568 // === INTERNAL FUNCTIONS ===
 569 void UHCI_CheckPortUpdate(void *Ptr)
 570 {
 571         tUHCI_Controller        *Host = Ptr;
 572         // Enable ports
 573         for( int i = 0; i < 2; i ++ )
 574         {
 575                  int    port = PORTSC1 + i*2;
 576                 Uint16  status;
 577
 578                 status = _InWord(Host, port);
 579                 // Check for port change
 580                 if( !(status & 0x0002) )        continue;
 581                 _OutWord(Host, port, 0x0002);
 582
 583                 // Check if the port is connected
 584                 if( !(status & 1) )
 585                 {
 586                         // Tell the USB code it's gone.
 587                         USB_DeviceDisconnected(Host->RootHub, i);
 588                         continue;
 589                 }
 590                 else
 591                 {
 592                         LOG("Port %i has something", i);
 593                         // Reset port (set bit 9)
 594                         LOG("Reset");
 595                         _OutWord(Host, port, 0x0200);
 596                         Time_Delay(50); // 50ms delay
 597                         _OutWord(Host, port, _InWord(Host, port) & ~0x0200);
 598                         // Enable port
 599                         LOG("Enable");
 600                         Time_Delay(50); // 50ms delay
 601                         _OutWord(Host, port, _InWord(Host, port) | 0x0004);
 602                         // Tell USB there's a new device
 603                         USB_DeviceConnected(Host->RootHub, i);
 604                 }
 605         }
 606 }
 607
 608 tUHCI_TD *UHCI_int_GetTDFromPhys(tUHCI_Controller *Controller, Uint32 PAddr)
 609 {
 610         if( PAddr >= Controller->PhysTDQHPage && PAddr < Controller->PhysTDQHPage + PAGE_SIZE )
 611         {
 612                 PAddr -= Controller->PhysTDQHPage;
 613                 PAddr -= (128+2)*sizeof(tUHCI_QH);
 614                 if( PAddr > PAGE_SIZE ) return NULL;    // Wrapping will bring above page size
 615                 PAddr /= sizeof(tUHCI_TD);
 616                 return &Controller->TDQHPage->LocalTDPool[PAddr];
 617         }
 618
 619
 620         tPAddr  global_pool = MM_GetPhysAddr( (tVAddr)gaUHCI_TDPool );
 621
 622         if( PAddr < global_pool || PAddr >= global_pool + PAGE_SIZE )   return NULL;
 623
 624         PAddr -= global_pool;
 625         PAddr /= sizeof(tUHCI_TD);
 626         return &gaUHCI_TDPool[PAddr];
 627 }
 628
 629 void UHCI_int_CleanQH(tUHCI_Controller *Cont, tUHCI_QH *QH)
 630 {
 631         tUHCI_TD        *td, *prev = NULL;
 632         Uint32  cur_td;
 633
 634         // Disable controller
 635         _OutWord( Cont, USBCMD, 0x0000 );
 636
 637         // Scan QH list
 638         cur_td = QH->Child;
 639         while( !(cur_td & 1) )
 640         {
 641                 td = UHCI_int_GetTDFromPhys(Cont, cur_td);
 642                 if(!td) {
 643                         Log_Warning("UHCI", "_int_CleanQH: QH %p contains TD %x, which was not from a pool",
 644                                 QH, cur_td);
 645                         break ;
 646                 }
 647
 648                 // Active? Ok.
 649                 if( td->Control & (1 << 23) ) {
 650                         prev = td;
 651                         continue ;
 652                 }
 653
 654                 LOG("Removed %p from QH %p", td, QH);
 655
 656                 if( !prev )
 657                         QH->Child = td->Link;
 658                 else
 659                         prev->Link = td->Link;
 660                 cur_td = td->Link;
 661         }
 662
 663         // re-enable controller
 664         _OutWord( Cont, USBCMD, 0x0001 );
 665 }
 666
 667 void UHCI_int_InterruptThread(void *Pointer)
 668 {
 669         tUHCI_Controller        *Cont = Pointer;
 670         Threads_SetName("UHCI Interrupt Handler");
 671         for( ;; )
 672         {
 673                 LOG("zzzzz....");
 674                 // 0 = Take all
 675                 Semaphore_Wait(&gUHCI_InterruptSempahore, 0);
 676                 LOG("Huh?");
 677
 678                 for( int i = 0; i < NUM_TDs; i ++ )
 679                 {
 680                          int    byte_count;
 681                         tUHCI_ExtraTDInfo       *info;
 682                         tUHCI_TD        *td;
 683
 684                         td = &gaUHCI_TDPool[i];
 685                         info = td->_info.ExtraInfo;
 686
 687                         // Skip completely inactive TDs
 688                         if( td->_info.bActive == 0 )    continue ;
 689                         // Skip ones that are still in use
 690                         if( td->Control & (1 << 23) )   continue ;
 691
 692                         // If no callback/alt buffer, mark as free and move on
 693                         if( td->_info.ExtraInfo )
 694                         {
 695                                 // Get size of transfer
 696                                 byte_count = (td->Control & 0x7FF)+1;
 697
 698                                 // Handle non page-aligned destination (or with a > 32-bit paddr)
 699                                 if(info->FirstPage)
 700                                 {
 701                                         char    *src, *dest;
 702                                          int    src_ofs = td->BufferPointer & (PAGE_SIZE-1);
 703                                         src = (void *) MM_MapTemp(td->BufferPointer);
 704                                         dest = (void *) MM_MapTemp(info->FirstPage);
 705                                         // Check for a single page transfer
 706                                         if( byte_count + info->Offset <= PAGE_SIZE )
 707                                         {
 708                                                 LOG("Single page copy %P to %P of %p",
 709                                                         td->BufferPointer, info->FirstPage, td);
 710                                                 memcpy(dest + info->Offset, src + src_ofs, byte_count);
 711                                         }
 712                                         else
 713                                         {
 714                                                 // Multi-page
 715                                                 LOG("Multi page copy %P to (%P,%P) of %p",
 716                                                         td->BufferPointer, info->FirstPage, info->SecondPage, td);
 717                                                  int    part_len = PAGE_SIZE - info->Offset;
 718                                                 memcpy(dest + info->Offset, src + src_ofs, part_len);
 719                                                 MM_FreeTemp( (tVAddr)dest );
 720                                                 dest = (void *) MM_MapTemp(info->SecondPage);
 721                                                 memcpy(dest, src + src_ofs + part_len, byte_count - part_len);
 722                                         }
 723                                         MM_FreeTemp( (tVAddr)src );
 724                                         MM_FreeTemp( (tVAddr)dest );
 725                                 }
 726
 727                                 // Callback
 728                                 if( info->Callback != NULL )
 729                                 {
 730                                         LOG("Calling cb %p", info->Callback);
 731                                         void    *ptr = (void *) MM_MapTemp( td->BufferPointer );
 732                                         info->Callback( info->CallbackPtr, ptr, byte_count );
 733                                         MM_FreeTemp( (tVAddr)ptr );
 734                                 }
 735
 736                                 // Clean up info
 737                                 free( info );
 738                                 td->_info.ExtraInfo = NULL;
 739                         }
 740
 741                         if( td->_info.QueueIndex <= 127 )
 742                         {
 743                                 LOG("Re-schedule interrupt %p (offset %i)", td, td->_info.QueueIndex);
 744                                 // TODO: Flip toggle?
 745                                 td->Control |= (1 << 23);
 746                                 // Add back into controller's interrupt list
 747                                 UHCI_int_AppendTD(
 748                                         Cont,
 749                                         Cont->TDQHPage->InterruptQHs + td->_info.QueueIndex,
 750                                         td
 751                                         );
 752                                 continue ;
 753                         }
 754
 755                         if( td->_info.bFreePointer )
 756                                 MM_DerefPhys( td->BufferPointer );
 757
 758                         // Error check
 759                         if( td->Control & 0x00FF0000 ) {
 760                                 LOG("td->control(Status) = %s%s%s%s%s%s%s%s",
 761                                         td->Control & (1 << 23) ? "Active " : "",
 762                                         td->Control & (1 << 22) ? "Stalled " : "",
 763                                         td->Control & (1 << 21) ? "Data Buffer Error " : "",
 764                                         td->Control & (1 << 20) ? "Babble " : "",
 765                                         td->Control & (1 << 19) ? "NAK " : "",
 766                                         td->Control & (1 << 18) ? "CRC Error, " : "",
 767                                         td->Control & (1 << 17) ? "Bitstuff Error, " : "",
 768                                         td->Control & (1 << 16) ? "Reserved " : ""
 769                                         );
 770                                 // Clean up QH (removing all inactive entries)
 771                                 UHCI_int_CleanQH(Cont, Cont->TDQHPage->InterruptQHs + td->_info.QueueIndex);
 772                         }
 773
 774                         // Clean up
 775                         LOG("Cleaned %p (->Control = %x)", td, td->Control);
 776                         td->_info.bActive = 0;
 777                 }
 778         }
 779 }
 780
 781 void UHCI_InterruptHandler(int IRQ, void *Ptr)
 782 {
 783         tUHCI_Controller *Host = Ptr;
 784 //       int    frame = (_InWord(Host, FRNUM) - 1) & 0x3FF;
 785         Uint16  status = _InWord(Host, USBSTS);
 786
 787         // Interrupt-on-completion
 788         if( status & 1 )
 789         {
 790                 // TODO: Support isochronous transfers (will need updating the frame pointer)
 791                 Semaphore_Signal(&gUHCI_InterruptSempahore, 1);
 792         }
 793
 794         LOG("status = 0x%04x", status);
 795         _OutWord(Host, USBSTS, status);
 796 }
 797
 798 void _OutByte(tUHCI_Controller *Host, int Reg, Uint8 Value)
 799 {
 800         if( Host->MemIOMap )
 801                 ((Uint8*)Host->MemIOMap)[Reg] = Value;
 802         else
 803                 outb(Host->IOBase + Reg, Value);
 804 }
 805
 806 void _OutWord(tUHCI_Controller *Host, int Reg, Uint16 Value)
 807 {
 808         if( Host->MemIOMap )
 809                 Host->MemIOMap[Reg/2] = Value;
 810         else
 811                 outw(Host->IOBase + Reg, Value);
 812 }
 813
 814 void _OutDWord(tUHCI_Controller *Host, int Reg, Uint32 Value)
 815 {
 816         if( Host->MemIOMap )
 817                 ((Uint32*)Host->MemIOMap)[Reg/4] = Value;
 818         else
 819                 outd(Host->IOBase + Reg, Value);
 820 }
 821
 822 Uint16 _InWord(tUHCI_Controller *Host, int Reg)
 823 {
 824         if( Host->MemIOMap )
 825                 return Host->MemIOMap[Reg/2];
 826         else
 827                 return inw(Host->IOBase + Reg);
 828 }
 829