Merge branch 'master' of git://localhost/acess2
[tpg/acess2.git] / KernelLand / Modules / USB / EHCI / ehci.c
1 /*
2  * Acess2 EHCI Driver
3  * - By John Hodge (thePowersGang)
4  * 
5  * ehci.c
6  * - Driver Core
7  */
8 #define DEBUG   0
9 #define VERSION VER2(0,1)
10 #include <acess.h>
11 #include <modules.h>
12 #include <usb_host.h>
13 #include "ehci.h"
14 #include <drv_pci.h>
15 #include <limits.h>
16 #include <events.h>
17 #include <timers.h>
18
19 // === CONSTANTS ===
20 #define EHCI_MAX_CONTROLLERS    4
21 #define EHCI_THREADEVENT_IOC    THREAD_EVENT_USER1
22 #define EHCI_THREADEVENT_PORTSC THREAD_EVENT_USER2
23 #define EHCI_THREADEVENT_IAAD   THREAD_EVENT_USER3
24
25 // === PROTOTYPES ===
26  int    EHCI_Initialise(char **Arguments);
27  int    EHCI_Cleanup(void);
28  int    EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum);
29 // -- API ---
30 void    *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bInput, int Period,
31         tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
32 void    *EHCI_InitIsoch  (void *Ptr, int Endpoint, size_t MaxPacketSize);
33 void    *EHCI_InitControl(void *Ptr, int Endpoint, size_t MaxPacketSize);
34 void    *EHCI_InitBulk   (void *Ptr, int Endpoint, size_t MaxPacketSize);
35 void    EHCI_RemEndpoint(void *Ptr, void *Handle);
36 void    *EHCI_SendControl(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData,
37         int isOutbound,
38         const void *SetupData, size_t SetupLength,
39         const void *OutData, size_t OutLength,
40         void *InData, size_t InLength
41         );
42 void    *EHCI_SendBulk(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData, int Dir, void *Data, size_t Length);
43 void    EHCI_FreeOp(void *Ptr, void *Handle);
44 Uint32  EHCI_int_RootHub_FeatToMask(int Feat);
45 void    EHCI_RootHub_SetPortFeature(void *Ptr, int Port, int Feat);
46 void    EHCI_RootHub_ClearPortFeature(void *Ptr, int Port, int Feat);
47  int    EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag);
48 // --- Internals ---
49 tEHCI_Endpoint  *EHCI_int_AllocateEndpt(tEHCI_Controller *Ctrlr, int Endpoint, size_t MPS, int PeriodPow2);
50 tEHCI_qTD       *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length);
51 void    EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD);
52 tEHCI_QH        *EHCI_int_AllocateQH(tEHCI_Controller *Cont, tEHCI_Endpoint *Endpoint, tUSBHostCb Cb, void *CbPtr);
53 void    EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH);
54 void    EHCI_InterruptHandler(int IRQ, void *Ptr);
55 // --- Interrupts ---
56 void    EHCI_int_AppendQHToAsync(tEHCI_Controller *Cont, tEHCI_QH *QH);
57 void    EHCI_int_InterruptThread(void *ControllerPtr);
58
59 // === GLOBALS ===
60 MODULE_DEFINE(0, VERSION, USB_EHCI, EHCI_Initialise, NULL, "USB_Core", NULL);
61 tEHCI_Controller        gaEHCI_Controllers[EHCI_MAX_CONTROLLERS];
62 tUSBHostDef     gEHCI_HostDef = {
63         .InitInterrupt = EHCI_InitInterrupt,
64         .InitIsoch     = EHCI_InitIsoch,
65         .InitControl   = EHCI_InitControl,
66         .InitBulk      = EHCI_InitBulk,
67         .RemEndpoint   = EHCI_RemEndpoint,
68         .SendIsoch   = NULL,
69         .SendControl = EHCI_SendControl,
70         .SendBulk    = EHCI_SendBulk,
71         .FreeOp      = EHCI_FreeOp,
72         
73         .CheckPorts = NULL,     // No need
74         .SetPortFeature   = EHCI_RootHub_SetPortFeature,
75         .ClearPortFeature = EHCI_RootHub_ClearPortFeature,
76         .GetPortStatus    = EHCI_RootHub_GetPortStatus,
77         };
78
79 // === CODE ===
80 int EHCI_Initialise(char **Arguments)
81 {
82         for( int id = -1; (id = PCI_GetDeviceByClass(0x0C0320, 0xFFFFFF, id)) >= 0;  )
83         {
84                 Uint32  addr = PCI_GetValidBAR(id, 0, PCI_BARTYPE_MEM);
85                 if( addr == 0 ) {
86                         Log_Error("EHCI", "PCI%i BAR0 is not memory", id);
87                         continue ;
88                 }
89                 Uint8   irq = PCI_GetIRQ(id);
90                 if( irq == 0 ) {
91                         Log_Error("EHCI", "PCI%i has no IRQ", id);
92                         continue ;
93                 }
94
95                 Log_Log("ECHI", "Controller at PCI %i 0x%x IRQ 0x%x", id, addr, irq);
96
97                 if( EHCI_InitController(addr, irq) )
98                 {
99                         // TODO: Detect other forms of failure than "out of slots"
100                         break ;
101                 }
102         }
103
104         for( int i = 0; Arguments && Arguments[i]; i ++ )
105         {
106                 char *pos = Arguments[i], *next;
107                 LOG("pos = '%s'", pos);
108                 tPAddr base = strtoull(pos, &next, 16);
109                 if( base == 0 )
110                         continue;
111                 pos = next;
112                 LOG("pos = '%s'", pos);
113                 if( *pos++ != '-' )
114                         continue;
115                 LOG("pos = '%s'", pos);
116                 int irq = strtol(pos, &next, 16);
117                 if( irq == 0 )
118                         continue ;
119                 if( *next != 0 )
120                         continue;
121                 LOG("base=%x, irq=%i", base, irq);
122                 if( EHCI_InitController(base, irq) )
123                 {
124                         continue ;
125                 }
126         }
127
128         return 0;
129 }
130
131 int EHCI_Cleanup(void)
132 {
133         return 0;
134 }
135
136 // --- Driver Init ---
137 int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
138 {
139         tEHCI_Controller        *cont = NULL;
140
141         ENTER("PBaseAddress iInterruptNum",
142                 BaseAddress, InterruptNum);
143
144         // Allocate a controller structure
145         for( int i = 0; i < EHCI_MAX_CONTROLLERS; i ++ )
146         {
147                 if( gaEHCI_Controllers[i].PhysBase == 0 ) {
148                         cont = &gaEHCI_Controllers[i];
149                         cont->PhysBase = BaseAddress;
150                         break;
151                 }
152         }
153         if(!cont) {
154                 Log_Notice("EHCI", "Too many controllers (EHCI_MAX_CONTROLLERS=%i)",
155                         EHCI_MAX_CONTROLLERS);
156                 LEAVE('i', 1);
157                 return 1;
158         }
159
160         // - Nuke a couple of fields so error handling code doesn't derp
161         cont->CapRegs = NULL;
162         cont->PeriodicQueue = NULL;
163         cont->TDPool = NULL;
164
165         // -- Build up structure --
166         cont->CapRegs = (void*)( MM_MapHWPages(BaseAddress, 1) + (BaseAddress % PAGE_SIZE) );
167         if( !cont->CapRegs ) {
168                 Log_Warning("EHCI", "Can't map 1 page at %P into kernel space", BaseAddress);
169                 goto _error;
170         }
171         LOG("cont->CapRegs = %p", cont->CapRegs);
172         if( (cont->CapRegs->CapLength & 3) ) {
173                 Log_Warning("EHCI", "Controller at %P non-aligned op regs (%x)",
174                         BaseAddress, cont->CapRegs->CapLength);
175                 goto _error;
176         }
177         if( BaseAddress % PAGE_SIZE + cont->CapRegs->CapLength + sizeof(tEHCI_CapRegs) > PAGE_SIZE ) {
178                 Log_Warning("EHCI", "%P: Cap regs over page boundary (+0x%x bytes)",
179                          BaseAddress % PAGE_SIZE + cont->CapRegs->CapLength + sizeof(tEHCI_CapRegs)
180                          );
181                 goto _error;
182         }
183         cont->OpRegs = (void*)( (Uint32*)cont->CapRegs + cont->CapRegs->CapLength / 4 );
184         LOG("cont->OpRegs = %p", cont->OpRegs);
185         // - Allocate periodic queue
186         tPAddr  unused;
187         cont->PeriodicQueue = (void*)MM_AllocDMA(1, 32, &unused);
188         if( !cont->PeriodicQueue ) {
189                 Log_Warning("ECHI", "Can't allocate 1 32-bit page for periodic queue");
190                 goto _error;
191         }
192         //  > Populate queue (with non-present entries)
193         for( int i = 0; i < 1024; i ++ )
194                 cont->PeriodicQueue[i] = 1;
195         // TODO: For 64-bit, QH pool and periodic queue need to be in the same 32-bit segment
196
197         // - Allocate TD pool
198         cont->TDPool = (void*)MM_AllocDMA(1, 32, &unused);
199         if( !cont->TDPool ) {
200                 Log_Warning("ECHI", "Can't allocate 1 32-bit page for qTD pool");
201                 goto _error;
202         }
203         for( int i = 0; i < TD_POOL_SIZE; i ++ ) {
204                 cont->TDPool[i].Token = 3 << 8; // TODO: what is this value?
205         }
206
207         // Get port count
208         cont->nPorts = cont->CapRegs->HCSParams & 0xF;
209         LOG("cont->nPorts = %i", cont->nPorts);
210
211         // -- Bind IRQ --
212         IRQ_AddHandler(InterruptNum, EHCI_InterruptHandler, cont);
213         cont->InterruptThread = Proc_SpawnWorker(EHCI_int_InterruptThread, cont);
214         if( !cont->InterruptThread ) {
215                 Log_Warning("EHCI", "Can't spawn interrupt worker thread");
216                 goto _error;
217         }
218         LOG("cont->InterruptThread = %p", cont->InterruptThread);
219
220         // Dummy TD
221         cont->DeadTD = EHCI_int_AllocateTD(cont, 0, NULL, 0);
222         memset(cont->DeadTD, 0, sizeof(tEHCI_qTD));
223         cont->DeadTD->Link = 1;
224         cont->DeadTD->Link2 = 1;
225         cont->DeadTD->Token = QTD_TOKEN_STS_HALT;
226         
227         // Dummy QH
228         // - HLink is set to itself, initing a circular list
229         cont->DeadQH = EHCI_int_AllocateQH(cont, NULL, NULL, NULL);
230         memset(cont->DeadQH, 0, sizeof(tEHCI_QH));
231         cont->DeadQH->HLink = MM_GetPhysAddr(cont->DeadQH)|2;
232         cont->DeadQH->Impl.Next = cont->DeadQH;
233         cont->DeadQH->Endpoint = QH_ENDPT_H;    // H - Head of Reclamation List
234         cont->DeadQH->CurrentTD = MM_GetPhysAddr(cont->DeadTD);
235         cont->DeadQH->Overlay.Link = MM_GetPhysAddr(cont->DeadTD);
236
237         // -- Initialisation procedure (from ehci-r10) --
238         // - Reset controller
239         cont->OpRegs->USBCmd = USBCMD_HCReset;
240         // - Set CTRLDSSEGMENT (TODO: 64-bit support)
241         // - Set USBINTR
242         cont->OpRegs->USBIntr = USBINTR_IOC|USBINTR_PortChange|USBINTR_FrameRollover|USBINTR_IntrAsyncAdvance;
243         // - Set PERIODICLIST BASE
244         cont->OpRegs->PeridocListBase = MM_GetPhysAddr( cont->PeriodicQueue );
245         // - Set ASYNCLISTADDR
246         cont->OpRegs->AsyncListAddr = MM_GetPhysAddr(cont->DeadQH);
247         // - Enable controller
248         cont->OpRegs->USBCmd = (0x40 << 16) | USBCMD_PeriodicEnable | USBCMD_AsyncEnable | USBCMD_Run;
249         // - Route all ports
250         cont->OpRegs->ConfigFlag = 1;
251
252         // -- Register with USB Core --
253         cont->RootHub = USB_RegisterHost(&gEHCI_HostDef, cont, cont->nPorts);
254         LOG("cont->RootHub = %p", cont->RootHub);
255
256         LEAVE('i', 0);
257         return 0;
258 _error:
259         cont->PhysBase = 0;
260         if( cont->CapRegs )
261                 MM_Deallocate( cont->CapRegs );
262         if( cont->PeriodicQueue )
263                 MM_Deallocate( cont->PeriodicQueue );
264         if( cont->TDPool )
265                 MM_Deallocate( cont->TDPool );
266         LEAVE('i', 2);
267         return 2;
268 }
269
270
271 static inline int _GetClosestPower2(int Period)
272 {
273         // Round the period to the closest power of two
274         int pow2period = 1;
275         int period_pow = 0;
276         // - Find the first power above the period
277         while( pow2period < Period )
278         {
279                 pow2period *= 2;
280                 period_pow ++;
281         }
282         // - Check which is closest
283         if( Period - pow2period / 2 > pow2period - Period ) {
284                 ;
285         }
286         else {
287                 period_pow --;
288         }
289         LOG("period_pow = %i (%ims) from %ims", period_pow, 1 << period_pow, Period);
290         return period_pow;
291 }
292
293 void EHCI_int_AddToPeriodic(tEHCI_Controller *Cont, tEHCI_QH *qh, int PeriodPow2, size_t Load)
294 {
295          int    Period = 1 << PeriodPow2;
296         Mutex_Acquire(&Cont->PeriodicListLock);
297
298         // Choose an interrupt slot to use      
299         int minslot = 0, minslot_load = INT_MAX;
300         for( int slot = 0; slot < Period; slot ++ )
301         {
302                  int    load = 0;
303                 for( int i = 0; i < PERIODIC_SIZE; i += Period )
304                         load += Cont->InterruptLoad[i+slot];
305                 if( load == 0 ) break;
306                 if( load < minslot_load ) {
307                         minslot = slot;
308                         minslot_load = load;
309                 }
310         }
311         // Increase loading on the selected slot
312         for( int i = minslot; i < PERIODIC_SIZE; i += Period )
313                 Cont->InterruptLoad[i] += Load;
314         qh->Impl.Endpt->InterruptOfs = minslot;
315         LOG("Using slot %i", minslot);
316
317         // Insert into the periodic list
318         for( int i = 0; i < PERIODIC_SIZE; i += Period )
319         {
320                 // Walk list until
321                 // - the end is reached
322                 // - this QH is found
323                 // - A QH with a lower period is encountered
324                 tEHCI_QH        *pqh = NULL;
325                 tEHCI_QH        *nqh;
326                 for( nqh = Cont->PeriodicQueueV[i]; nqh; pqh = nqh, nqh = nqh->Impl.Next )
327                 {
328                         if( nqh == qh )
329                                 break;
330                         if( nqh->Impl.Endpt->PeriodPow2 < PeriodPow2 )
331                                 break;
332                 }
333
334                 // Somehow, we've already been added to this queue.
335                 if( nqh && nqh == qh )
336                         continue ;
337
338                 if( qh->Impl.Next && qh->Impl.Next != nqh ) {
339                         Log_Warning("EHCI", "Suspected bookkeeping error on %p - int list %i+%i overlap",
340                                 Cont, PeriodPow2, minslot);
341                         break;
342                 }
343
344                 if( nqh ) {
345                         qh->Impl.Next = nqh;
346                         qh->HLink = MM_GetPhysAddr(nqh) | 2;
347                 }
348                 else {
349                         qh->Impl.Next = NULL;
350                         qh->HLink = 2|1;        // QH, Terminate
351                 }
352
353                 if( pqh ) {
354                         pqh->Impl.Next = qh;
355                         pqh->HLink = MM_GetPhysAddr(qh) | 2;
356                 }
357                 else {
358                         Cont->PeriodicQueueV[i] = qh;
359                         Cont->PeriodicQueue[i] = MM_GetPhysAddr(qh) | 2;
360                 }
361         }
362         Mutex_Release(&Cont->PeriodicListLock);
363 }
364
365 // --------------------------------------------------------------------
366 // USB API
367 // --------------------------------------------------------------------
368 void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
369         tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
370 {
371         tEHCI_Controller        *Cont = Ptr;
372         
373         ASSERTCR(Endpoint, <, 256*16, NULL);
374         ASSERTCR(Period, >, 0, NULL);
375         if( Period > 256 )
376                 Period = 256;
377
378         LOG("Endpoint=%x, bOutbound=%i, Period=%i, Length=%i", Endpoint, bOutbound, Period, Length);
379
380         int period_pow = _GetClosestPower2(Period);
381
382         tEHCI_Endpoint  *endpt = EHCI_int_AllocateEndpt(Cont, Endpoint, Length, period_pow);
383         endpt->Next = Cont->FirstInterruptEndpt;
384         Cont->FirstInterruptEndpt = endpt;
385
386         // Allocate a QH
387         tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, endpt, Cb, CbData);
388         qh->EndpointExt |= 1;   // TODO: uFrame load balancing (8 entry bitfield)
389
390         // Allocate TD for the data
391         tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (bOutbound ? PID_OUT : PID_IN), Buf, Length);
392         qh->CurrentTD = MM_GetPhysAddr(td);
393         qh->Impl.FirstTD = td;
394         qh->Impl.LastTD = td;
395
396         // - Insert into period queue
397         EHCI_int_AddToPeriodic(Cont, qh, period_pow, Length);
398
399         return endpt;
400 }
401
402 void *EHCI_InitIsoch(void *Ptr, int Endpoint, size_t MaxPacketSize)
403 {
404         ENTER("pPtr iEndpoint iMaxPacketSize",
405                 Ptr, Endpoint, MaxPacketSize);
406         LEAVE_RET('p', (void*)(tVAddr)(Endpoint + 1));
407 }
408 void *EHCI_InitControl(void *Ptr, int Endpoint, size_t MaxPacketSize)
409 {
410         return EHCI_int_AllocateEndpt(Ptr, Endpoint, MaxPacketSize, -1);
411 }
412 void *EHCI_InitBulk(void *Ptr, int Endpoint, size_t MaxPacketSize)
413 {
414         return EHCI_int_AllocateEndpt(Ptr, Endpoint, MaxPacketSize, -2);
415 }
416 void EHCI_RemEndpoint(void *Ptr, void *Handle)
417 {
418         if( Handle == NULL )
419                 return ;
420         else if( (tVAddr)Handle <= 256*16 )
421                 return ;        // Isoch
422         else {
423                 tEHCI_Endpoint  *endpt = Ptr;
424
425                 // Remove QH from list
426                 // - If it's a polling endpoint, need to remove from all periodic lists
427                 if( endpt->PeriodPow2 >= 0) {
428                         // Poll
429                 }
430                 else {
431                         // Control/Bulk
432                 }
433                 
434                 // Deallocate endpoint
435                 // TODO: Write EHCI_DeallocateEndpoint
436                 free(endpt);
437         }
438 }
439
440 void *EHCI_SendControl(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData,
441         int isOutbound,
442         const void *SetupData, size_t SetupLength,
443         const void *OutData, size_t OutLength,
444         void *InData, size_t InLength
445         )
446 {
447         tEHCI_Controller *Cont = Ptr;
448         tEHCI_Endpoint  *endpt = Dest;
449         tEHCI_QH        *qh;
450         tEHCI_qTD       *td_setup, *td_data, *td_status;
451
452         // Sanity checks
453         if( (tVAddr)Dest <= 256*16 )
454                 return NULL;
455         if( endpt->PeriodPow2 != -1 ) {
456                 Log_Notice("EHCI", "Non-control endpoint passed to SendControl");
457                 return NULL;
458         }
459         if( SetupLength > endpt->MaxPacketSize )
460                 return NULL;
461         // TODO: Check size of status
462         
463
464         LOG("Dest=%p, isOutbound=%i, Lengths(Setup:%i,Out:%i,In:%i)",
465                 Dest, isOutbound, SetupLength, OutLength, InLength);
466
467         // Allocate a QH to work with
468         qh = EHCI_int_AllocateQH(Cont, endpt, Cb, CbData);
469
470         // Allocate TDs
471         td_setup = EHCI_int_AllocateTD(Cont, PID_SETUP, (void*)SetupData, SetupLength);
472         if( isOutbound )
473         {
474                 td_data = OutData ? EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength) : NULL;
475                 td_status = EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength);
476         }
477         else
478         {
479                 td_data = InData ? EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength) : NULL;
480                 td_status = EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength);
481         }
482         td_status->Token |= QTD_TOKEN_IOC;      // IOC
483
484         // Append TDs
485         if( td_data ) {
486                 td_setup->Link = MM_GetPhysAddr(td_data);
487                 td_data->Link = MM_GetPhysAddr(td_status);
488                 td_data->Token |= QTD_TOKEN_STS_ACTIVE; // Active
489         }
490         else {
491                 td_setup->Link = MM_GetPhysAddr(td_status);
492         }
493         td_setup->Token |= QTD_TOKEN_STS_ACTIVE;        // Active
494         td_status->Token |= QTD_TOKEN_STS_ACTIVE;
495         td_status->Link = 1;
496         td_status->Link2 = 1;
497         
498         // Set QH's current pointer
499         qh->CurrentTD = MM_GetPhysAddr(td_setup);
500         qh->Impl.FirstTD = td_setup;
501         qh->Impl.LastTD = td_status;
502         EHCI_int_AppendQHToAsync(Cont, qh);
503
504         return qh;
505 }
506
507 void *EHCI_SendBulk(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData, int Dir, void *Data, size_t Length)
508 {
509         tEHCI_Controller        *Cont = Ptr;
510         tEHCI_Endpoint  *endpt = Dest;
511         
512         // Sanity check the pointer
513         // - Can't be NULL or an isoch
514         if( (tVAddr)Dest <= 256*16 )
515                 return NULL;
516         if( endpt->PeriodPow2 != -2 ) {
517                 return NULL;
518         }
519         LOG("Ptr=%p,Dest=%p(%x),Cb=%p(%p),DirIsOut=%i,Data=%p+0x%x)",
520                 Ptr, Dest, endpt->EndpointID, Cb, CbData, Dir, Data, Length);
521
522         // Allocate a QH to work with
523         tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, endpt, Cb, CbData);
524         
525         // Allocate single TD
526         tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (Dir ? PID_OUT : PID_IN), Data, Length);
527         td->Token |= QTD_TOKEN_IOC | QTD_TOKEN_STS_ACTIVE;
528         qh->CurrentTD = MM_GetPhysAddr(td);
529         qh->Impl.FirstTD = td;
530         qh->Impl.LastTD = td;
531         
532         EHCI_int_AppendQHToAsync(Cont, qh);
533
534         return qh;
535 }
536
537 void EHCI_FreeOp(void *Ptr, void *Handle)
538 {
539         tEHCI_Controller        *Cont = Ptr;
540
541         EHCI_int_DeallocateQH(Cont, Handle);
542 }
543
544 Uint32 EHCI_int_RootHub_FeatToMask(int Feat)
545 {
546         switch(Feat)
547         {
548         case PORT_RESET:        return PORTSC_PortReset;
549         case PORT_ENABLE:       return PORTSC_PortEnabled;
550         default:
551                 Log_Warning("EHCI", "Unknown root hub port feature %i", Feat);
552                 return 0;
553         }
554 }
555
556 void EHCI_RootHub_SetPortFeature(void *Ptr, int Port, int Feat)
557 {
558         tEHCI_Controller        *Cont = Ptr;
559         if(Port >= Cont->nPorts)        return;
560
561         Cont->OpRegs->PortSC[Port] |= EHCI_int_RootHub_FeatToMask(Feat);
562 }
563
564 void EHCI_RootHub_ClearPortFeature(void *Ptr, int Port, int Feat)
565 {
566         tEHCI_Controller        *Cont = Ptr;
567         if(Port >= Cont->nPorts)        return;
568
569         Cont->OpRegs->PortSC[Port] &= ~EHCI_int_RootHub_FeatToMask(Feat);
570 }
571
572 int EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag)
573 {
574         tEHCI_Controller        *Cont = Ptr;
575         if(Port >= Cont->nPorts)        return 0;
576
577         return !!(Cont->OpRegs->PortSC[Port] & EHCI_int_RootHub_FeatToMask(Flag));
578 }
579
580 // --------------------------------------------------------------------
581 // Internals
582 // --------------------------------------------------------------------
583 tEHCI_Endpoint *EHCI_int_AllocateEndpt(tEHCI_Controller *Cont, int Endpoint, size_t MaxPacketSize, int PeriodPow2)
584 {
585         ENTER("pCont iEndpoint iMaxPacketSize",
586                 Cont, Endpoint, MaxPacketSize);
587         
588         tEHCI_Endpoint  *endpt = malloc( sizeof(*endpt) );
589         endpt->NextToggle = FALSE;
590         endpt->EndpointID = Endpoint;
591         endpt->MaxPacketSize = MaxPacketSize;   // TODO: Sanity Check
592         endpt->PeriodPow2 = PeriodPow2;
593         endpt->InterruptOfs = 0;
594         endpt->ActiveQHs = NULL;
595         // TODO: store endpoints on controller
596         
597         LOG("Created %p for %P Ep 0x%x - %i bytes MPS",
598                 endpt, Cont->PhysBase, Endpoint, MaxPacketSize);
599
600         LEAVE('p', endpt);
601         return endpt;
602 }
603
604 tEHCI_qTD *EHCI_int_GetTDFromPhys(tEHCI_Controller *Cont, Uint32 Addr)
605 {
606         if( Addr == 0 ) return NULL;
607         LOG("%p + (%x - %x)", Cont->TDPool, Addr, MM_GetPhysAddr(Cont->TDPool));
608         return Cont->TDPool + (Addr - MM_GetPhysAddr(Cont->TDPool))/sizeof(tEHCI_qTD);
609 }
610
611 tEHCI_qTD *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length)
612 {
613 //      Semaphore_Wait(&Cont->TDSemaphore, 1);
614         Mutex_Acquire(&Cont->TDPoolMutex);
615         for( int i = 0; i < TD_POOL_SIZE; i ++ )
616         {
617                 // PID code == 3: Avaliable for use
618                 if( ((Cont->TDPool[i].Token >> 8) & 3) != 3 )
619                         continue ;
620                 
621                 Cont->TDPool[i].Token = (PID << 8) | (Length << 16);
622                 Mutex_Release(&Cont->TDPoolMutex);
623                 
624                 tEHCI_qTD       *td = &Cont->TDPool[i];
625                 td->Impl.Ptr = Data;
626                 td->Impl.Length = Length;
627                 td->Link = 1;
628                 td->Link2 = 1;
629                 
630                 // TODO: Support breaking across multiple pages
631                 ASSERTC(Length, <, PAGE_SIZE);
632                 // TODO: Handle bouncing >32-bit pages
633                 #if PHYS_BITS > 32
634                 ASSERT( MM_GetPhysAddr(Data) >> 32 == 0 );
635                 #endif
636                 td->Pages[0] = MM_GetPhysAddr(Data);
637                 if( (td->Pages[0] & (PAGE_SIZE-1)) + Length - 1 > PAGE_SIZE )
638                         td->Pages[1] = MM_GetPhysAddr((char*)Data + Length - 1) & ~(PAGE_SIZE-1);
639                 LOG("Allocated %p(%P) PID%i on %P (Token=0x%x)",
640                         td, MM_GetPhysAddr(td), PID, Cont->PhysBase,
641                         td->Token);
642                 return td;
643         }
644
645         Mutex_Release(&Cont->TDPoolMutex);
646         return NULL;
647 }
648
649 void EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD)
650 {
651         UNIMPLEMENTED();
652 }
653
654 tEHCI_QH *EHCI_int_AllocateQH(tEHCI_Controller *Cont, tEHCI_Endpoint *Endpoint, tUSBHostCb Cb, void *CbData)
655 {
656         tEHCI_QH        *ret;
657         Mutex_Acquire(&Cont->QHPoolMutex);
658         for( int i = 0; i < QH_POOL_SIZE; i ++ )
659         {
660                 // If page not yet allocated, allocate it
661                 if( !MM_GetPhysAddr( Cont->QHPools[i/QH_POOL_NPERPAGE] ) ) {
662                         tPAddr  tmp;
663                         Cont->QHPools[i/QH_POOL_NPERPAGE] = (void*)MM_AllocDMA(1, 32, &tmp);
664                         memset(Cont->QHPools[i/QH_POOL_NPERPAGE], 0, PAGE_SIZE);
665                 }
666
667                 ret = &Cont->QHPools[i/QH_POOL_NPERPAGE][i%QH_POOL_NPERPAGE];
668                 if( ret->HLink != 0 )
669                         continue ;
670                 
671                 memset(ret, 0, sizeof(*ret));
672                 ret->HLink = 1;
673                 Mutex_Release(&Cont->QHPoolMutex);
674                 
675                 if( Endpoint )
676                 {
677                         ret->Endpoint = (Endpoint->EndpointID >> 4) | 0x80 | ((Endpoint->EndpointID & 0xF) << 8)
678                                 | (Endpoint->MaxPacketSize << 16);
679                 }
680                 ret->EndpointExt = (1<<30);
681                 ret->Impl.Next = NULL;
682                 ret->Impl.Callback = Cb;
683                 ret->Impl.CallbackData = CbData;
684                 ret->Impl.Endpt = Endpoint;
685                 // TODO: Endpoint speed (13:12) 0:Full, 1:Low, 2:High
686                 // TODO: Control Endpoint Flag (27) 0:*, 1:Full/Low Control
687                 return ret;
688         }
689         Mutex_Release(&Cont->QHPoolMutex);
690         return NULL;
691 }
692
693 void EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH)
694 {
695         UNIMPLEMENTED();
696         // TODO: Ensure it's unused and clean up associated TDs
697         QH->HLink = 0;
698 }
699
700 void EHCI_int_AppendQHToAsync(tEHCI_Controller *Cont, tEHCI_QH *QH)
701 {
702         QH->Overlay.Token = QTD_TOKEN_STS_ACTIVE;
703         
704         Mutex_Acquire(&Cont->lAsyncSchedule);
705         // Insert into list
706         QH->HLink = Cont->DeadQH->HLink;
707         QH->Impl.Next = Cont->DeadQH->Impl.Next;
708         Cont->DeadQH->HLink = MM_GetPhysAddr(QH)|2;
709         Cont->DeadQH->Impl.Next = QH;
710         // Start async schedule
711         // - Set ASYNCENABLE
712         // - Clear 'H' in dead QH
713         Mutex_Release(&Cont->lAsyncSchedule);
714         LOG("Appended %P to %p(%P)", MM_GetPhysAddr(QH), Cont, Cont->PhysBase);
715 }
716
717 // --------------------------------------------------------------------
718 // Interrupt actions
719 // --------------------------------------------------------------------
720 void EHCI_InterruptHandler(int IRQ, void *Ptr)
721 {
722         tEHCI_Controller *Cont = Ptr;
723         Uint32  sts = Cont->OpRegs->USBSts;
724         Uint32  orig_sts = sts;
725         const Uint32    reserved_bits = 0xFFFF0FC0;
726
727         if( sts & reserved_bits ) {
728                 LOG("Oops, reserved bits set (%08x), funny hardware?", sts);
729                 sts &= ~reserved_bits;
730         }
731
732         // Unmask read-only bits
733         sts &= ~(0xF000);
734
735         if( sts & USBINTR_IOC ) {
736                 // IOC
737                 LOG("%P IOC", Cont->PhysBase);
738                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_IOC);
739                 sts &= ~USBINTR_IOC;
740         }
741
742         if( sts & USBINTR_IntrAsyncAdvance ) {
743                 LOG("%P IAAD", Cont->PhysBase);
744                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_IAAD);
745                 sts &= ~USBINTR_IntrAsyncAdvance;
746         }
747
748         if( sts & USBINTR_PortChange ) {
749                 // Port change, determine what port and poke helper thread
750                 LOG("%P Port status change", Cont->PhysBase);
751                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_PORTSC);
752                 sts &= ~USBINTR_PortChange;
753         }
754         
755         if( sts & USBINTR_FrameRollover ) {
756                 // Frame rollover, used to aid timing (trigger per-second operations)
757                 //LOG("%p Frame rollover", Ptr);
758                 sts &= ~USBINTR_FrameRollover;
759         }
760
761         if( sts ) {
762                 // Unhandled interupt bits
763                 // TODO: Warn
764                 LOG("WARN - Bitmask %x unhandled", sts);
765         }
766
767
768         // Clear interrupts
769         Cont->OpRegs->USBSts = orig_sts;
770 }
771
772 void EHCI_int_HandlePortConnectChange(tEHCI_Controller *Cont, int Port)
773 {
774         // Connect Event
775         if( Cont->OpRegs->PortSC[Port] & PORTSC_CurrentConnectStatus )
776         {
777                 // Is the device low-speed?
778                 if( (Cont->OpRegs->PortSC[Port] & PORTSC_LineStatus_MASK) == PORTSC_LineStatus_Kstate )
779                 {
780                         LOG("Low speed device on %P Port %i, giving to companion", Cont->PhysBase, Port);
781                         Cont->OpRegs->PortSC[Port] |= PORTSC_PortOwner;
782                 }
783                 // not a low-speed device, EHCI reset
784                 else
785                 {
786                         LOG("Device connected on %P Port %i", Cont->PhysBase, Port);
787                         // Reset procedure.
788                         USB_PortCtl_BeginReset(Cont->RootHub, Port);
789                 }
790         }
791         // Disconnect Event
792         else
793         {
794                 if( Cont->OpRegs->PortSC[Port] & PORTSC_PortOwner ) {
795                         LOG("Device disconnected on %P Port %i (companion), taking it back",
796                                 Cont->PhysBase, Port);
797                         Cont->OpRegs->PortSC[Port] &= ~PORTSC_PortOwner;
798                 }
799                 else {
800                         LOG("Device disconnected on %P Port %i", Cont->PhysBase, Port);
801                         USB_DeviceDisconnected(Cont->RootHub, Port);
802                 }
803         }
804 }
805
806 void EHCI_int_CheckInterruptQHs(tEHCI_Controller *Cont)
807 {
808         for( tEHCI_Endpoint *endpt = Cont->FirstInterruptEndpt; endpt; endpt = endpt->Next )
809         {
810                 tEHCI_QH *qh = endpt->ActiveQHs;
811                 // Check if the TD of the first QH is active
812                 if( qh->Overlay.Token & QTD_TOKEN_STS_ACTIVE )
813                         continue ;
814                 // Inactive, fire interrupt and re-trigger
815                 if( !qh->Impl.Callback ) {
816                         // Umm... ?
817                 }
818                 else
819                 {
820                         tEHCI_qTD       *last = qh->Impl.LastTD;
821                         size_t  remaining_len = (last->Token >> 16) & 0x7FFF;
822                         if( remaining_len > last->Impl.Length )
823                                 remaining_len = last->Impl.Length;
824                         size_t  transferred_len = last->Impl.Length - remaining_len;
825                         
826                         LOG("Calling %p(%p) for Intr EndPt %x (%p+0x%x)",
827                                 qh->Impl.Callback, qh->Impl.CallbackData,
828                                 qh->Impl.Endpt->EndpointID, last->Impl.Ptr, transferred_len);
829                         qh->Impl.Callback(qh->Impl.CallbackData, last->Impl.Ptr, transferred_len);
830                 }
831                 qh->Impl.FirstTD->Token |= QTD_TOKEN_STS_ACTIVE;
832                 qh->Overlay.Token |= QTD_TOKEN_STS_ACTIVE;
833         }
834 }
835
836 void EHCI_int_RetireQHs(tEHCI_Controller *Cont)
837 {
838         tEHCI_QH        *prev = Cont->DeadQH;
839         Mutex_Acquire(&Cont->lAsyncSchedule);
840         for( tEHCI_QH *qh = prev->Impl.Next; qh != Cont->DeadQH; qh = prev->Impl.Next )
841         {
842                 ASSERT(qh);
843                 ASSERT(qh != qh->Impl.Next);
844                 if( qh->Overlay.Token & QTD_TOKEN_STS_ACTIVE ) {
845                         prev = qh;
846                         continue ;
847                 }
848                 
849                 // Remove from async list
850                 prev->HLink = qh->HLink;
851                 prev->Impl.Next = qh->Impl.Next;
852                 
853                 // Add to reclaim list
854                 qh->Impl.Next = Cont->ReclaimList;
855                 Cont->ReclaimList = qh;
856                 
857                 // Ring doorbell!
858                 Cont->OpRegs->USBCmd |= USBCMD_IAAD;
859         }
860         Mutex_Release(&Cont->lAsyncSchedule);
861 }
862
863 /*
864  * Fire callbacks on QHs and mark them as completed
865  * 
866  * TODO: Possible bug with items being added to reclaim list before
867  *       the last doorbell fires.
868  */
869 void EHCI_int_ReclaimQHs(tEHCI_Controller *Cont)
870 {
871         // Doorbell was rung, so reclaim QHs
872         // - Actually just fires callbacks, now that we know that the QHs can be cleared
873         tEHCI_QH        *qh;
874         Mutex_Acquire(&Cont->lReclaimList);
875         while( (qh = Cont->ReclaimList) )
876         {
877                 Cont->ReclaimList = qh->Impl.Next;
878                 Mutex_Release(&Cont->lReclaimList);
879
880                 if( qh->Impl.Callback )
881                 {
882                         tEHCI_qTD       *last = qh->Impl.LastTD;
883                         size_t  remaining_len = (last->Token >> 16) & 0x7FFF;
884                         if( remaining_len > last->Impl.Length )
885                                 remaining_len = last->Impl.Length;
886                         size_t  transferred_len = last->Impl.Length - remaining_len;
887                         
888                         LOG("Calling %p(%p) for EndPt %x (%p+0x%x)",
889                                 qh->Impl.Callback, qh->Impl.CallbackData,
890                                 qh->Impl.Endpt->EndpointID, last->Impl.Ptr, transferred_len);
891                         //if( last->Impl.Ptr )
892                         //      Debug_HexDump("EHCI Callback Data", last->Impl.Ptr, transferred_len);
893                         qh->Impl.Callback(qh->Impl.CallbackData, last->Impl.Ptr, transferred_len);
894                 }
895                 
896                 Mutex_Acquire(&Cont->lReclaimList);
897         }
898         Mutex_Release(&Cont->lReclaimList);
899 }
900
901 void EHCI_int_InterruptThread(void *ControllerPtr)
902 {
903         tEHCI_Controller        *Cont = ControllerPtr;
904         Threads_SetName("EHCI Interrupt Worker");
905         while(Cont->OpRegs)
906         {
907                 Uint32  events;
908                 
909                 LOG("sleeping for events");
910                 events = Threads_WaitEvents(EHCI_THREADEVENT_IOC|EHCI_THREADEVENT_PORTSC|EHCI_THREADEVENT_IAAD);
911                 if( !events ) {
912                         // TODO: Should this cause a termination?
913                 }
914                 LOG("events = 0x%x", events);
915
916                 if( events & EHCI_THREADEVENT_IAAD )
917                 {
918                         EHCI_int_ReclaimQHs(Cont);
919                 }
920
921                 if( events & EHCI_THREADEVENT_IOC )
922                 {
923                         // IOC, handle completed requests
924                         // Search periodic lists for one that fired
925                         EHCI_int_CheckInterruptQHs(Cont);
926                         // Retire QHs
927                         // - Remove them from the queue and ask the controller to bell when they're removable
928                         EHCI_int_RetireQHs(Cont);
929                 }
930
931                 // Port status change interrupt
932                 if( events & EHCI_THREADEVENT_PORTSC )
933                 {
934                         // Check for port status changes
935                         for(int i = 0; i < Cont->nPorts; i ++ )
936                         {
937                                 Uint32  sts = Cont->OpRegs->PortSC[i];
938                                 //LOG("Port %i: sts = %x", i, sts);
939                                 Cont->OpRegs->PortSC[i] = sts;
940                                 if( sts & PORTSC_ConnectStatusChange )
941                                 {
942                                         LOG("Port %i: Connect Change", i);
943                                         EHCI_int_HandlePortConnectChange(Cont, i);
944                                 }
945
946                                 if( sts & PORTSC_PortEnableChange )
947                                 {
948                                         // Handle enable/disable
949                                         LOG("Port %i: Enable Change", i);
950                                 }
951
952                                 if( sts & PORTSC_OvercurrentChange )
953                                 {
954                                         // Handle over-current change
955                                         LOG("Port %i: Over-Current Change", i);
956                                 }
957                         }
958                 }
959         }
960 }

UCC git Repository :: git.ucc.asn.au