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         // Sanity check Endpt
 554         if( (tVAddr)Endpt > 0x7FF ) {
 555                 LEAVE('n');
 556                 return NULL;
 557         }
 558         // if( Cont->Devs[Dest/16] == NULL )    LEAVE_RET('n', NULL);
 559         // if( Cont->Devs[Dest/16].EndPt[Dest%16].Type != 1 )   LEAVE_RET('n', NULL);
 560         // MAX_PACKET_SIZE = Cont->Devs[Dest/16].EndPt[Dest%16].MPS;
 561         // Tgl = Cont->Devs[Dest/16].EndPt[Dest%16].Tgl;
 562
 563         // TODO: Build up list and then append to QH in one operation
 564
 565         char    *data_ptr, *status_ptr;
 566         size_t  data_len,   status_len;
 567         Uint8   data_pid,   status_pid;
 568
 569         if( bOutbound ) {
 570                 data_pid   = PID_OUT; data_ptr   = (void*)OutData; data_len   = OutLength;
 571                 status_pid = PID_IN;  status_ptr = InData;  status_len = InLength;
 572         }
 573         else {
 574                 data_pid   = PID_IN;  data_ptr   = InData;  data_len   = InLength;
 575                 status_pid = PID_OUT; status_ptr = (void*)OutData; status_len = OutLength;
 576         }
 577
 578         // Sanity check data lengths
 579         if( SetupLength > MAX_PACKET_SIZE )     LEAVE_RET('n', NULL);
 580         if( status_len > MAX_PACKET_SIZE )      LEAVE_RET('n', NULL);
 581
 582         // Create and append SETUP packet
 583         td = UHCI_int_CreateTD(Cont, Dest, PID_SETUP, Tgl, NULL, NULL, (void*)SetupData, SetupLength);
 584         UHCI_int_AppendTD(Cont, qh, td);
 585         Tgl = !Tgl;
 586
 587         // Data packets
 588         while( data_len > 0 )
 589         {
 590                 size_t len = MIN(data_len, MAX_PACKET_SIZE);
 591                 td = UHCI_int_CreateTD(Cont, Dest, data_pid, Tgl, NULL, NULL, data_ptr, len);
 592                 UHCI_int_AppendTD(Cont, qh, td);
 593                 Tgl = !Tgl;
 594
 595                 data_ptr += len;
 596                 data_len -= len;
 597                 // TODO: Handle multi-packet
 598         }
 599
 600         td = UHCI_int_CreateTD(Cont, Dest, status_pid, Tgl, Cb, CbData, status_ptr, status_len);
 601         UHCI_int_AppendTD(Cont, qh, td);
 602         Tgl = !Tgl;
 603
 604         // Cont->Devs[Dest/16].EndPt[Dest%16].Tgl = !Tgl;
 605
 606         LEAVE('p', td);
 607         return td;
 608 }
 609
 610 void *UHCI_SendBulk(void *Ptr, void *Endpt, tUSBHostCb Cb, void *CbData, int bOutbound, void *Data, size_t Length)
 611 {
 612         tUHCI_Controller        *Cont = Ptr;
 613         tUHCI_QH        *qh = &Cont->TDQHPage->BulkQH;
 614         tUHCI_TD        *td = NULL;
 615          int    Dest = (int)Endpt - 1;
 616          int    Tgl = 0;
 617
 618         ENTER("pPtr pEndpt pCb pCbData bOutbound pData iLength", Ptr, Dest, Cb, CbData, bOutbound, Data, Length);
 619
 620         // TODO: Validation
 621         // TODO: Data toggle
 622
 623         Uint8   pid = (bOutbound ? PID_OUT : PID_IN);
 624
 625         char *pos = Data;
 626         while( Length > 0 )
 627         {
 628                 size_t len = MIN(MAX_PACKET_SIZE, Length);
 629
 630                 td = UHCI_int_CreateTD(Cont, Dest, pid, Tgl, Cb, (len == Length ? CbData : NULL), pos, len);
 631                 UHCI_int_AppendTD(Cont, qh, td);
 632
 633                 pos += len;
 634                 Length -= len;
 635                 Tgl = !Tgl;
 636         }
 637
 638         LEAVE('p', td);
 639         return td;
 640 }
 641
 642 // ==========================
 643 // === INTERNAL FUNCTIONS ===
 644 // ==========================
 645 void UHCI_CheckPortUpdate(void *Ptr)
 646 {
 647         tUHCI_Controller        *Host = Ptr;
 648         // Enable ports
 649         for( int i = 0; i < 2; i ++ )
 650         {
 651                  int    port = PORTSC1 + i*2;
 652                 Uint16  status;
 653
 654                 status = _InWord(Host, port);
 655                 // Check for port change
 656                 if( !(status & 0x0002) )        continue;
 657                 _OutWord(Host, port, 0x0002);
 658
 659                 // Check if the port is connected
 660                 if( !(status & 1) )
 661                 {
 662                         // Tell the USB code it's gone.
 663                         USB_DeviceDisconnected(Host->RootHub, i);
 664                         continue;
 665                 }
 666                 else
 667                 {
 668                         LOG("Port %i has something", i);
 669                         // Reset port (set bit 9)
 670                         LOG("Reset");
 671                         _OutWord(Host, port, 0x0200);
 672                         Time_Delay(50); // 50ms delay
 673                         _OutWord(Host, port, _InWord(Host, port) & ~0x0200);
 674                         // Enable port
 675                         LOG("Enable");
 676                         Time_Delay(50); // 50ms delay
 677                         _OutWord(Host, port, _InWord(Host, port) | 0x0004);
 678                         // Tell USB there's a new device
 679                         USB_DeviceConnected(Host->RootHub, i);
 680                 }
 681         }
 682 }
 683
 684 tUHCI_TD *UHCI_int_GetTDFromPhys(tUHCI_Controller *Controller, Uint32 PAddr)
 685 {
 686         if( PAddr >= Controller->PhysTDQHPage && PAddr < Controller->PhysTDQHPage + PAGE_SIZE )
 687         {
 688                 PAddr -= Controller->PhysTDQHPage;
 689                 PAddr -= (128+2)*sizeof(tUHCI_QH);
 690                 if( PAddr > PAGE_SIZE ) return NULL;    // Wrapping will bring above page size
 691                 PAddr /= sizeof(tUHCI_TD);
 692                 return &Controller->TDQHPage->LocalTDPool[PAddr];
 693         }
 694
 695
 696         tPAddr  global_pool = MM_GetPhysAddr( gaUHCI_TDPool );
 697
 698         if( PAddr < global_pool || PAddr >= global_pool + PAGE_SIZE )   return NULL;
 699
 700         PAddr -= global_pool;
 701         PAddr /= sizeof(tUHCI_TD);
 702         return &gaUHCI_TDPool[PAddr];
 703 }
 704
 705 void UHCI_int_CleanQH(tUHCI_Controller *Cont, tUHCI_QH *QH)
 706 {
 707         tUHCI_TD        *td, *prev = NULL;
 708         Uint32  cur_td;
 709          int    nCleaned = 0;
 710
 711         // Disable controller
 712         _OutWord( Cont, USBCMD, 0x0000 );
 713
 714         // Scan QH list
 715         cur_td = QH->Child;
 716         LOG("cur_td = 0x%08x", cur_td);
 717         while( !(cur_td & 1) )
 718         {
 719                 td = UHCI_int_GetTDFromPhys(Cont, cur_td);
 720                 if(!td) {
 721                         Log_Warning("UHCI", "_int_CleanQH: QH %p contains TD %x, which was not from a pool",
 722                                 QH, cur_td);
 723                         break ;
 724                 }
 725
 726                 // Active? Ok.
 727                 if( td->Control & TD_CTL_ACTIVE ) {
 728                         LOG("%p still active", td);
 729                         prev = td;
 730                         cur_td = td->Link;
 731                         continue ;
 732                 }
 733
 734                 LOG("Removed %p from QH %p", td, QH);
 735
 736                 if( !prev )
 737                         QH->Child = td->Link;
 738                 else
 739                         prev->Link = td->Link;
 740                 cur_td = td->Link;
 741                 nCleaned ++;
 742         }
 743
 744         if( nCleaned == 0 ) {
 745                 LOG("Nothing cleaned... what the?");
 746         }
 747
 748         // re-enable controller
 749         _OutWord( Cont, USBCMD, 0x0001 );
 750 }
 751
 752 void UHCI_int_HandleTDComplete(tUHCI_Controller *Cont, tUHCI_TD *TD)
 753 {
 754          int    byte_count = (TD->Control & 0x7FF)+1;
 755         tUHCI_ExtraTDInfo       *info = TD->_info.ExtraInfo;
 756
 757         // Handle non page-aligned destination (or with a > 32-bit paddr)
 758         // TODO: Needs fixing for alignment issues
 759         if(info->FirstPage)
 760         {
 761                 char    *src, *dest;
 762                  int    src_ofs = TD->BufferPointer & (PAGE_SIZE-1);
 763                 src = MM_MapTemp(TD->BufferPointer);
 764                 dest = MM_MapTemp(info->FirstPage);
 765                 // Check for a single page transfer
 766                 if( byte_count + info->Offset <= PAGE_SIZE )
 767                 {
 768                         LOG("Single page copy %P to %P of %p",
 769                                 TD->BufferPointer, info->FirstPage, TD);
 770                         memcpy(dest + info->Offset, src + src_ofs, byte_count);
 771                 }
 772                 else
 773                 {
 774                         // Multi-page
 775                         LOG("Multi page copy %P to (%P,%P) of %p",
 776                                 TD->BufferPointer, info->FirstPage, info->SecondPage, TD);
 777                          int    part_len = PAGE_SIZE - info->Offset;
 778                         memcpy(dest + info->Offset, src + src_ofs, part_len);
 779                         MM_FreeTemp( dest );
 780                         dest = MM_MapTemp(info->SecondPage);
 781                         memcpy(dest, src + src_ofs + part_len, byte_count - part_len);
 782                 }
 783                 MM_FreeTemp( src );
 784                 MM_FreeTemp( dest );
 785         }
 786
 787         // Callback
 788         if( info->Callback != NULL )
 789         {
 790                 LOG("Calling cb %p (%i bytes)", info->Callback, byte_count);
 791                 void    *ptr = MM_MapTemp( TD->BufferPointer );
 792                 info->Callback( info->CallbackPtr, ptr, byte_count );
 793                 MM_FreeTemp( ptr );
 794         }
 795
 796         // Clean up info
 797         if( TD->_info.QueueIndex > 127 )
 798         {
 799                 free( info );
 800                 TD->_info.ExtraInfo = NULL;
 801         }
 802 }
 803
 804 void UHCI_int_InterruptThread(void *Pointer)
 805 {
 806         tUHCI_Controller        *Cont = Pointer;
 807         Threads_SetName("UHCI Interrupt Handler");
 808         for( ;; )
 809         {
 810                  int    nSeen = 0;
 811
 812                 LOG("zzzzz....");
 813                 // 0 = Take all
 814                 Semaphore_Wait(&gUHCI_InterruptSempahore, 0);
 815                 LOG("Huh?");
 816
 817                 for( int i = 0; i < NUM_TDs; i ++ )
 818                 {
 819                         tUHCI_TD        *td;
 820
 821                         td = &gaUHCI_TDPool[i];
 822
 823                         // Skip completely inactive TDs
 824                         if( td->_info.bActive == 0 )    continue ;
 825                         // Skip ones that are still in use
 826                         if( td->Control & TD_CTL_ACTIVE )       continue ;
 827
 828                         nSeen ++;
 829
 830                         // If no callback/alt buffer, mark as free and move on
 831                         if( td->_info.ExtraInfo )
 832                         {
 833                                 UHCI_int_HandleTDComplete(Cont, td);
 834                         }
 835
 836                         // Error check
 837                         if( td->Control & 0x00FF0000 ) {
 838                                 LOG("td->control(Status) = %s%s%s%s%s%s%s%s",
 839                                         td->Control & TD_CTL_ACTIVE     ? "Active, " : "",
 840                                         td->Control & TD_CTL_STALLED    ? "Stalled, " : "",
 841                                         td->Control & TD_CTL_DATABUFERR ? "Data Buffer Error, " : "",
 842                                         td->Control & TD_CTL_BABBLE     ? "Babble, " : "",
 843                                         td->Control & TD_CTL_NAK        ? "NAK, " : "",
 844                                         td->Control & TD_CTL_CRCERR     ? "CRC Error, " : "",
 845                                         td->Control & TD_CTL_BITSTUFF   ? "Bitstuff Error, " : "",
 846                                         td->Control & TD_CTL_RESERVED   ? "Reserved " : ""
 847                                         );
 848                                 LOG("From queue %i", td->_info.QueueIndex);
 849                                 // Clean up QH (removing all inactive entries)
 850                                 UHCI_int_CleanQH(Cont, Cont->TDQHPage->InterruptQHs + td->_info.QueueIndex);
 851                                 td->Control = 0;
 852                         }
 853
 854                         // Handle rescheduling of interrupt TDs
 855                         if( td->_info.QueueIndex <= 127 )
 856                         {
 857                                 LOG("Re-schedule interrupt %p (offset %i)", td, td->_info.QueueIndex);
 858                                 // TODO: Flip toggle?
 859                                 td->Control |= TD_CTL_ACTIVE;
 860                                 // Add back into controller's interrupt list
 861                                 UHCI_int_AppendTD(
 862                                         Cont,
 863                                         Cont->TDQHPage->InterruptQHs + td->_info.QueueIndex,
 864                                         td
 865                                         );
 866                                 continue ;
 867                         }
 868
 869                         // Clean up
 870                         if( td->_info.bFreePointer )
 871                                 MM_DerefPhys( td->BufferPointer );
 872
 873                         // Clean up
 874                         LOG("Cleaned %p (->Control = %x)", td, td->Control);
 875                         td->_info.bActive = 0;
 876                 }
 877
 878                 if( nSeen == 0 ) {
 879                         LOG("Why did you wake me?");
 880                 }
 881         }
 882 }
 883
 884 void UHCI_InterruptHandler(int IRQ, void *Ptr)
 885 {
 886         tUHCI_Controller *Host = Ptr;
 887 //       int    frame = (_InWord(Host, FRNUM) - 1) & 0x3FF;
 888         Uint16  status = _InWord(Host, USBSTS);
 889
 890         LOG("%p: status = 0x%04x", Ptr, status);
 891
 892         // Interrupt-on-completion
 893         if( status & 1 )
 894         {
 895                 // TODO: Support isochronous transfers (will need updating the frame pointer)
 896                 Semaphore_Signal(&gUHCI_InterruptSempahore, 1);
 897         }
 898
 899         // USB Error Interrupt
 900         if( status & 2 )
 901         {
 902
 903         }
 904
 905         // Resume Detect
 906         // - Fired if in suspend state and a USB device sends the RESUME signal
 907         if( status & 4 )
 908         {
 909
 910         }
 911
 912         // Host System Error
 913         if( status & 8 )
 914         {
 915
 916         }
 917
 918         // Host Controller Process Error
 919         if( status & 0x10 )
 920         {
 921                 Log_Error("UHCI", "Host controller process error on controller %p", Ptr);
 922         }
 923
 924         _OutWord(Host, USBSTS, status);
 925 }
 926
 927 void _OutByte(tUHCI_Controller *Host, int Reg, Uint8 Value)
 928 {
 929         if( Host->MemIOMap )
 930                 ((Uint8*)Host->MemIOMap)[Reg] = Value;
 931         else
 932                 outb(Host->IOBase + Reg, Value);
 933 }
 934
 935 void _OutWord(tUHCI_Controller *Host, int Reg, Uint16 Value)
 936 {
 937         if( Host->MemIOMap )
 938                 Host->MemIOMap[Reg/2] = Value;
 939         else
 940                 outw(Host->IOBase + Reg, Value);
 941 }
 942
 943 void _OutDWord(tUHCI_Controller *Host, int Reg, Uint32 Value)
 944 {
 945         if( Host->MemIOMap )
 946                 ((Uint32*)Host->MemIOMap)[Reg/4] = Value;
 947         else
 948                 outd(Host->IOBase + Reg, Value);
 949 }
 950
 951 Uint16 _InWord(tUHCI_Controller *Host, int Reg)
 952 {
 953         if( Host->MemIOMap )
 954                 return Host->MemIOMap[Reg/2];
 955         else
 956                 return inw(Host->IOBase + Reg);
 957 }
 958