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