Modules/USB - Bugfixes

[tpg/acess2.git] / Modules / USB / Core / usb_poll.c

diff --git a/Modules/USB/Core/usb_poll.c b/Modules/USB/Core/usb_poll.c
index a4e8d44..11afc13 100644 (file)
--- a/Modules/USB/Core/usb_poll.c
+++ b/Modules/USB/Core/usb_poll.c
@@ -26,19 +26,27 @@ void USB_StartPollingEndpoint(tUSBInterface *Iface, int Endpoint)
        tUSBEndpoint    *endpt;
 
        // Some sanity checks
-       if(Endpoint <= 0 || Endpoint >= Iface->nEndpoints)      return ;
+       if(Endpoint <= 0 || Endpoint > Iface->nEndpoints)       return ;
        endpt = &Iface->Endpoints[Endpoint-1];
        if(endpt->PollingPeriod > POLL_MAX || endpt->PollingPeriod <= 0)
                return ;
 
        // TODO: Check that this endpoint isn't already on the queue
 
+       endpt->InputData = malloc(endpt->MaxPacketSize);
+
        // Determine polling period in atoms
        endpt->PollingAtoms = (endpt->PollingPeriod + POLL_ATOM-1) / POLL_ATOM;
        if(endpt->PollingAtoms > POLL_SLOTS)    endpt->PollingAtoms = POLL_SLOTS;
        // Add to poll queue
-       endpt->Next = gUSB_PollQueues[endpt->PollingAtoms];
-       gUSB_PollQueues[endpt->PollingAtoms] = endpt;
+       // TODO: Locking
+       {
+                int    idx = giUSB_PollPosition + 1;
+               if(idx >= POLL_SLOTS)   idx -= POLL_SLOTS;
+               LOG("idx = %i", idx);
+               endpt->Next = gUSB_PollQueues[idx];
+               gUSB_PollQueues[idx] = endpt;
+       }
 }
 
 /**
@@ -46,6 +54,7 @@ void USB_StartPollingEndpoint(tUSBInterface *Iface, int Endpoint)
  */
 int USB_PollThread(void *unused)
 {
+       Threads_SetName("USB Polling Thread");
        for(;;)
        {
                tUSBEndpoint    *ep, *prev;
@@ -54,9 +63,11 @@ int USB_PollThread(void *unused)
                prev = (void*)( (tVAddr)&gUSB_PollQueues[giUSB_PollPosition] - offsetof(tUSBEndpoint, Next) );
 
                // Process queue
+//             LOG("giUSB_PollPosition = %i", giUSB_PollPosition);
                for( ep = gUSB_PollQueues[giUSB_PollPosition]; ep; prev = ep, ep = ep->Next )
                {
                         int    period_in_atoms = ep->PollingAtoms;
+                       LOG("%i: ep = %p", giUSB_PollPosition, ep);
 
                        // Check for invalid entries
                        if(period_in_atoms < 0 || period_in_atoms > POLL_ATOM)
@@ -78,6 +89,12 @@ int USB_PollThread(void *unused)
                        // TODO: Check the endpoint
                        // TODO: Async checking?
                        // - Send the read request on all of them then wait for the first to complete
+                       USB_RecvDataA(
+                               ep->Interface, ep->EndpointIdx+1,
+                               ep->MaxPacketSize, ep->InputData,
+                               ep->Interface->Driver->Endpoints[ep->EndpointIdx].DataAvail
+                               );
+                               
                        // Call callback
 
                        // Reschedule
@@ -90,6 +107,7 @@ int USB_PollThread(void *unused)
                                
                                ep->Next = *newqueue;
                                *newqueue = ep;
+                               ep = prev;
                        }
                }
                giUSB_PollPosition ++;