KernelLand/Modules/USB/UHCI/uhci.c

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