HRESULT PXA271_USB_Driver::Initialize( int Controller )
{
    int endpointsUsed = 0;
    const USB_ENDPOINT_DESCRIPTOR  *ep    = NULL;
    const USB_INTERFACE_DESCRIPTOR *itfc  = NULL;
    USB_CONTROLLER_STATE &State = UsbControllerState[0];

    ASSERT( Controller == 0 );

    GLOBAL_LOCK(irq);

    PXA271_USB& USB = PXA271::USB();

    // make sure clock is enabled
    PXA271::CLKMNGR().CKEN |= PXA271_CLOCK_MANAGER::CKEN__USB_CLIENT_48MHZ;

    // Enable the only interrupt
    CPU_INTC_ActivateInterrupt( PXA271_AITC::c_IRQ_INDEX_USB_CLIENT, Global_ISR, NULL );

    for( int i = 0; i < c_Used_Endpoints; i++ )
        EndpointInit[i].word = 0;       // All useable endpoints initialize to unused

    // For all endpoints in the USB configuration
    while( USB_NextEndpoint( &State, ep, itfc) )
    {
        UINT8 endpointNum   = ep->bEndpointAddress & 0x7F;
        UINT16 endpointSize = ep->wMaxPacketSize;
        
        // Check interface and endpoint numbers against hardware capability
        if( endpointNum >= c_Used_Endpoints || itfc->bInterfaceNumber > 7 )
            return S_FALSE;
        
        EndpointInit[endpointNum].bits.EN   = endpointNum;
        EndpointInit[endpointNum].bits.ED   = (ep->bEndpointAddress & 0x80) ? 1 : 0;
        EndpointInit[endpointNum].bits.IN   = itfc->bInterfaceNumber;
        EndpointInit[endpointNum].bits.ET   = ep->bmAttributes & 0x03;
        EndpointInit[endpointNum].bits.CN   = 1;        // Always only 1 configuration = 1
        EndpointInit[endpointNum].bits.AISN = 0;        // No alternate interfaces
        EndpointInit[endpointNum].bits.EE   = 1;        // Enable this endpoint

        // Set the maximum size of the endpoint hardware FIFO
        if( (ep->bmAttributes & 0x03) == USB_ENDPOINT_ATTRIBUTE_BULK )
        {
            // If the endpoint maximum size in the configuration list is bogus
            if( endpointSize != 8 && endpointSize != 16 && endpointSize != 32 && endpointSize != 64 )
                return S_FALSE;
            EndpointInit[endpointNum].bits.MPS = endpointSize;
            State.MaxPacketSize[endpointNum]   = endpointSize;
        }
        else if( (ep->bmAttributes & 0x03) == USB_ENDPOINT_ATTRIBUTE_INTERRUPT )
        {
            if( endpointSize == 0 || endpointSize > 64 )
                return S_FALSE;
            EndpointInit[endpointNum].bits.MPS = endpointSize;
            State.MaxPacketSize[endpointNum]   = endpointSize;
        }
        else        // Isochronous endpoint
        {
            if( endpointSize > 64 )
                endpointSize = 64;
            EndpointInit[endpointNum].bits.MPS = endpointSize;
            State.MaxPacketSize[endpointNum]   = endpointSize;
        }

        // Since endpoint 0 is only used for control, there is never a need to allocate a buffer for it
        // In fact State.Queues[0] is always NULL - it is a cheap placeholder to make the queueIndex = endpointIndex
        QueueBuffers[endpointNum-1].Initialize();                       // Clear queue before use
        State.Queues[endpointNum] = &QueueBuffers[endpointNum-1];       // Attach queue to endpoint

        // Set up direction information
        if( EndpointInit[endpointNum].bits.ED )         // If transmit endpoint
        {
            EndpointInit[endpointNum].bits.DE = 1;      // Only transmit is double buffered
            State.IsTxQueue[endpointNum]      = TRUE;
        }
        else        // If receive endpoint
        {
            EndpointInit[endpointNum].bits.DE = 0;
            State.IsTxQueue[endpointNum]      = FALSE;
        }
        endpointsUsed++;
    }
    
    // If no endpoints were initialized, something is seriously wrong with the configuration list
    if( 0 == endpointsUsed )
    {
        CPU_INTC_DeactivateInterrupt( PXA271_AITC::c_IRQ_INDEX_USB_CLIENT );
        return S_FALSE;
    }

    g_PXA271_USB_Driver.pUsbControllerState  = &State;
    g_PXA271_USB_Driver.PinsProtected        = TRUE;

    State.EndpointStatus = &g_PXA271_USB_Driver.EndpointStatus[0];
    State.EndpointCount  = c_Used_Endpoints;
    //State->DeviceStatus   = USB_STATUS_DEVICE_SELF_POWERED;
    State.PacketSize     = c_default_ctrl_packet_size;
    
    State.FirstGetDescriptor = TRUE;

    ProtectPins( Controller, FALSE );

    return S_OK;
}
// ---------------------------------------------------------------------------
HRESULT CPU_USB_Initialize(int core)
{
    if (core >= MAX_USB_CORE || USB_ENABLED(core)) return S_FALSE;

    USB_CONTROLLER_STATE *State = CPU_USB_GetState(core);
    const USB_ENDPOINT_DESCRIPTOR  *pEpDesc;
    const USB_INTERFACE_DESCRIPTOR *pIfDesc;
    int epNum, ret;
    UINT32 queueId = 0;

    GLOBAL_LOCK(irq);

    // Initialize physical layer
    if (!USB_ENABLED(1 - core))  // No other USB controllers enabled?
    {
        LPC_CGU->PLL[CGU_USB_PLL].PLL_CTRL &= ~1; // Enable USB PLL
        while (!(LPC_CGU->PLL[CGU_USB_PLL].PLL_STAT & 1)); // Wait for USB PLL to lock
    }
    LPC_CGU->BASE_CLK[USB_CLK[core]] &= ~1;  // Enable USBx base clock
    LPC_CREG->CREG0 &= ~(1 << 5); // Enable USB0 PHY
    if (core == 1) // Special init for USB1
    {
        // Enable USB1_DP and USB1_DN on chip FS phy
        LPC_SCU->SFSUSB = 0x12; // USB device mode (0x16 for host mode)
        LPC_USB1->PORTSC1_D |= (1 << 24);
    }

    // Set USB state
    USB_STATE(core) = State;
    USB_FLAGS(core) = 0;
    ep_out_count[core] = 0;
    ep_in_count[core] = 0;

    State->EndpointStatus = USB_EPSTATUS(core);
    State->EndpointCount = USB_MAX_EP_NUM;
    State->PacketSize = MAX_EP0_SIZE;

    // Initialize USB stack
    ret = USB_InitStack(core);
    if (ret != RET_OK) return S_FALSE; // Exit if not succesful

    // Set defaults for unused endpoints
    for (epNum = 1; epNum < State->EndpointCount; epNum++)
    {
        State->IsTxQueue[epNum] = FALSE;
        State->MaxPacketSize[epNum] = MAX_EP_SIZE;
    }

    // Get endpoint configuration
    while (USB_NextEndpoint(State, pEpDesc, pIfDesc))
    {
        // Figure out which endpoint we are initializing
        epNum = pEpDesc->bEndpointAddress & 0x7F;

        // Check interface and endpoint numbers against hardware capability
        if (epNum >= State->EndpointCount || pIfDesc->bInterfaceNumber > 3)
            return S_FALSE;

        if (pEpDesc->bEndpointAddress & 0x80) State->IsTxQueue[epNum] = TRUE;

        // Set the maximum size of the endpoint hardware FIFO
        int endpointSize = pEpDesc->wMaxPacketSize;
        // Exit if the endpoint maximum size in the configuration list is bogus
        // or greater than USB_MAX_DATA_PACKET_SIZE (default=64)
        if ((endpointSize != 8 && endpointSize != 16 && endpointSize != 32 && endpointSize != 64
                && endpointSize != 128 && endpointSize != 256 && endpointSize != 512)
				|| endpointSize > USB_MAX_DATA_PACKET_SIZE)
            return S_FALSE;
        State->MaxPacketSize[epNum] = endpointSize;

        // Assign queues
        QueueBuffers[queueId].Initialize();           // Clear queue before use
        State->Queues[epNum] = &QueueBuffers[queueId];  // Attach queue to endpoint
        queueId++;

        // Isochronous endpoints are currently not supported
        if ((pEpDesc->bmAttributes & 3) == USB_ENDPOINT_ATTRIBUTE_ISOCHRONOUS) return FALSE;
    }

    // Configure CDC driver
    ret = CDC_Init(core);
    if (ret != RET_OK) return S_FALSE; // Exit if not succesful

    // Connect and enable interrupts
    CPU_USB_ProtectPins(core, FALSE);
    CPU_INTC_ActivateInterrupt(USB_IRQ[core], USB_ISR[core], 0);
    USB_ENABLED(core) = TRUE;
    State->DeviceState = USB_DEVICE_STATE_CONFIGURED; // Config done by ROM stack
    USB_StateCallback(State);

    return S_OK;
}
HRESULT AT91_USBHS_Driver::Initialize( int Controller )
{
    
    ASSERT(0 == Controller);

    //UINT8 logicalEndpoint;
    UINT8 endpointNum;
    //UINT8 altInterface;
    USB_CONTROLLER_STATE     &State = UsbControllerState[Controller];
    const USB_ENDPOINT_DESCRIPTOR  *ep    = NULL;
    const USB_INTERFACE_DESCRIPTOR *itfc  = NULL;

    struct AT91_UDPHS *pUdp = (struct AT91_UDPHS *) (AT91C_BASE_UDP);
    struct AT91_UDPHS_EPT *pEp = (struct AT91_UDPHS_EPT *) (AT91C_BASE_UDP + 0x100);
    ep_dir[0]=0;
    ep_dir[1]=0;
    ep_dir[2]=0;
    ep_dir[3]=0;
    ep_dir[4]=0;
    ep_dir[5]=0;
    
    num_ep_int1= 0;
    num_ep_int2= 0;

    GLOBAL_LOCK(irq);
    
    
    // Enable USB device clock
    AT91_PMC_EnableUSBClock();
    AT91_PMC_EnableUTMIBIAS();

    // Enable the interrupt for  UDP
    CPU_INTC_ActivateInterrupt( AT91C_ID_UDP, Global_ISR, NULL);

    pUdp->UDPHS_IEN |= AT91C_UDPHS_EPT_INT_0;
    pEp->UDPHS_EPTCFG |= 0x00000043; //configuration info for control ep
//    pUdp->UDP_CSR[0] |= AT91C_UDP_EPTYPE_CTRL;

    // For all endpoints in the USB Configuration list
    //logicalEndpoint = 1;
    while( USB_NextEndpoint( &State, ep, itfc) ) // && logicalEndpoint < 11 )
    {
        // Figure out which endpoint we are initializing
        endpointNum = ep->bEndpointAddress & 0x7F;
        
        // Check interface and endpoint numbers against hardware capability
        if( endpointNum >= AT91_USBHS_Driver::c_Used_Endpoints || itfc->bInterfaceNumber > 3 )
            return S_FALSE;

/*        
        EndpointInit[logicalEndpoint].bits.EPNUM      = endpointNum;
        EndpointInit[logicalEndpoint].bits.FIFONUM    = endpointNum;
        EndpointInit[logicalEndpoint].bits.DIR        = (ep->bEndpointAddress & 0x80) ? 1 : 0;
        EndpointInit[logicalEndpoint].bits.INTERFACE  = itfc->bInterfaceNumber;
        EndpointInit[logicalEndpoint].bits.TYPE       = ep->bmAttributes & 0x03;
        EndpointInit[logicalEndpoint].bits.TRXTYP     = 3;      // Always 3 for non-control endpoints
        EndpointInit[logicalEndpoint].bits.CONFIG     = 1;      // Always only 1 configuration: #1
*/
        // Set the maximum size of the endpoint hardware FIFO
        if( (ep->bmAttributes & 0x03) == USB_ENDPOINT_ATTRIBUTE_BULK )
        {
            int endpointSize = ep->wMaxPacketSize;

            // If the endpoint maximum size in the configuration list is bogus
            if( endpointSize != 8 && endpointSize != 16 && endpointSize != 32 && endpointSize != 64 && endpointSize != 128 && endpointSize != 512 && endpointSize != 1024 )
                return S_FALSE;
            // If too large an endpoint size was requested
            if( endpointSize > s_EpAttr[endpointNum].Payload )
                return S_FALSE;
//            EndpointInit[logicalEndpoint].bits.MAXPKTSIZE = endpointSize;
            State.MaxPacketSize[endpointNum] = endpointSize;
        }
        else
        {
            // If Isochronous or Interupt type endpoint, always use the maximum size
//            EndpointInit[logicalEndpoint].bits.MAXPKTSIZE = s_EpAttr[endpointNum].Payload;
            State.MaxPacketSize[endpointNum] = s_EpAttr[endpointNum].Payload;
        }

        // Since endpoint 0 is only used for control, there is never a need to allocate a buffer for it
        // In fact State.Queues[0] is always NULL - it is a cheap placeholder to make the queueIndex = endpointIndex
        QueueBuffers[endpointNum-1].Initialize();                       // Clear queue before use
        State.Queues[endpointNum] = &QueueBuffers[endpointNum-1];       // Attach queue to endpoint
        
        
        
        pEp = (struct AT91_UDPHS_EPT *) (AT91C_BASE_UDP + 0x100 + 0x20 * (endpointNum));
        // Enable an interrupt for the endpoint & set its direction
        if( (ep->bEndpointAddress & 0x80) ? 1 : 0 )        // If transmit endpoint
        {
            State.IsTxQueue[endpointNum] = TRUE;
            
            switch(ep->bmAttributes & 0x03)
            {
                case 1:
                    pEp->UDPHS_EPTCFG |= AT91C_UDPHS_EPT_TYPE_ISO_EPT|AT91C_UDPHS_EPT_DIR_OUT;
                    
                    break;
                case 2:
                    pEp->UDPHS_EPTCFG |= AT91C_UDPHS_EPT_TYPE_BUL_EPT|AT91C_UDPHS_EPT_DIR_OUT;
                    
                    break;
                case 3:
                    pEp->UDPHS_EPTCFG |= AT91C_UDPHS_EPT_TYPE_INT_EPT|AT91C_UDPHS_EPT_DIR_OUT;
                    
                    break;
            }
            
        }
        else        // Receive endpoint
        {
            State.IsTxQueue[endpointNum] = FALSE;
            switch(ep->bmAttributes & 0x03)
            {
                case 1:
                    pEp->UDPHS_EPTCFG |= AT91C_UDPHS_EPT_TYPE_ISO_EPT|AT91C_UDPHS_EPT_DIR_IN;
                    
                    break;
                case 2:
                    pEp->UDPHS_EPTCFG |= AT91C_UDPHS_EPT_TYPE_BUL_EPT|AT91C_UDPHS_EPT_DIR_IN;
                    
                    break;
                case 3:
                    pEp->UDPHS_EPTCFG |= AT91C_UDPHS_EPT_TYPE_INT_EPT|AT91C_UDPHS_EPT_DIR_IN;
                    
                    break;
            }
        }
        
                
        pUdp->UDPHS_IEN |= (AT91C_UDPHS_EPT_INT_0 << endpointNum);

        // Move onto the next logical endpoint
        //logicalEndpoint++;
    }

    g_AT91_USBHS_Driver.pUsbControllerState  = &State;
    g_AT91_USBHS_Driver.PinsProtected = TRUE;

    State.EndpointStatus = &g_AT91_USBHS_Driver.EndpointStatus[0];
    State.EndpointCount  = c_Used_Endpoints;
    State.PacketSize     = s_EpAttr[0].Payload;

    State.FirstGetDescriptor = TRUE;
    //State.DeviceState = USB_DEVICE_STATE_DETACHED;

    ProtectPins( Controller, FALSE );
//    debug_printf("ep_dir[1]: %x, ep_dir[2]: %x, %d, %d\r\n", ep_dir[1], ep_dir[2], State.MaxPacketSize[1], State.MaxPacketSize[2]);
    

    return S_OK;
}