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