/** @file
The module to produce Usb Bus PPI.

Copyright (c) 2006 - 2018, Intel Corporation. All rights reserved.<BR>

This program and the accompanying materials
are licensed and made available under the terms and conditions
of the BSD License which accompanies this distribution.  The
full text of the license may be found at
http://opensource.org/licenses/bsd-license.php

THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.

**/

#include "UsbPeim.h"
#include "HubPeim.h"
#include "PeiUsbLib.h"

//
// UsbIo PPI interface function
//
PEI_USB_IO_PPI         mUsbIoPpi = {
  PeiUsbControlTransfer,
  PeiUsbBulkTransfer,
  PeiUsbGetInterfaceDescriptor,
  PeiUsbGetEndpointDescriptor,
  PeiUsbPortReset
};

EFI_PEI_PPI_DESCRIPTOR mUsbIoPpiList = {
  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
  &gPeiUsbIoPpiGuid,
  NULL
};

/**
  The enumeration routine to detect device change.

  @param  PeiServices            Describes the list of possible PEI Services.
  @param  UsbHcPpi               The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.
  @param  Usb2HcPpi              The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.

  @retval EFI_SUCCESS            The usb is enumerated successfully.
  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
  @retval Others                 Other failure occurs.

**/
EFI_STATUS
PeiUsbEnumeration (
  IN EFI_PEI_SERVICES               **PeiServices,
  IN PEI_USB_HOST_CONTROLLER_PPI    *UsbHcPpi,
  IN PEI_USB2_HOST_CONTROLLER_PPI    *Usb2HcPpi
  );

/**
  Configure new detected usb device.

  @param  PeiServices            Describes the list of possible PEI Services.
  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.
  @param  Port                   The port to be configured.
  @param  DeviceAddress          The device address to be configured.

  @retval EFI_SUCCESS            The new detected usb device is configured successfully.
  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
  @retval Others                 Other failure occurs.

**/
EFI_STATUS
PeiConfigureUsbDevice (
  IN     EFI_PEI_SERVICES    **PeiServices,
  IN     PEI_USB_DEVICE      *PeiUsbDevice,
  IN     UINT8               Port,
  IN OUT UINT8               *DeviceAddress
  );

/**
  Get all configurations from a detected usb device.

  @param  PeiServices            Describes the list of possible PEI Services.
  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.

  @retval EFI_SUCCESS            The new detected usb device is configured successfully.
  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
  @retval Others                 Other failure occurs.

**/
EFI_STATUS
PeiUsbGetAllConfiguration (
  IN EFI_PEI_SERVICES   **PeiServices,
  IN PEI_USB_DEVICE     *PeiUsbDevice
  );

/**
  Get the start position of next wanted descriptor.

  @param  Buffer            Buffer containing data to parse.
  @param  Length            Buffer length.
  @param  DescType          Descriptor type.
  @param  DescLength        Descriptor length.
  @param  ParsedBytes       Bytes has been parsed.

  @retval EFI_SUCCESS       Get wanted descriptor successfully.
  @retval EFI_DEVICE_ERROR  Error occurred.

**/
EFI_STATUS
GetExpectedDescriptor (
  IN  UINT8       *Buffer,
  IN  UINTN       Length,
  IN  UINT8       DescType,
  IN  UINT8       DescLength,
  OUT UINTN       *ParsedBytes
  );

/**
  The entrypoint of the module, it will enumerate all HCs.

  @param  FileHandle             Handle of the file being invoked.
  @param  PeiServices            Describes the list of possible PEI Services.

  @retval EFI_SUCCESS            Usb initialization is done successfully.
  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
  @retval EFI_UNSUPPORTED        Can't find required PPI.

**/
EFI_STATUS
EFIAPI
PeimInitializeUsb (
  IN EFI_PEI_FILE_HANDLE        FileHandle,
  IN CONST EFI_PEI_SERVICES     **PeiServices
  )
{
  EFI_STATUS                   Status;
  UINTN                        Index;
  PEI_USB_HOST_CONTROLLER_PPI  *UsbHcPpi;
  PEI_USB2_HOST_CONTROLLER_PPI *Usb2HcPpi;

  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
    return EFI_SUCCESS;
  }

  //
  // gPeiUsbHostControllerPpiGuid and gPeiUsb2HostControllerPpiGuid should not
  // be produced at the same time
  //
  Index = 0;
  while (TRUE) {
    //
    // Get UsbHcPpi at first.
    //
    Status = PeiServicesLocatePpi (
               &gPeiUsbHostControllerPpiGuid,
               Index,
               NULL,
               (VOID **) &UsbHcPpi
               );
    if (EFI_ERROR (Status)) {
      //
      // No more host controller, break out
      //
      break;
    }
    PeiUsbEnumeration ((EFI_PEI_SERVICES **) PeiServices, UsbHcPpi, NULL);
    Index++;
  }

  if (Index == 0) {
    //
    // Then try to get Usb2HcPpi.
    //
    while (TRUE) {
      Status = PeiServicesLocatePpi (
                 &gPeiUsb2HostControllerPpiGuid,
                 Index,
                 NULL,
                 (VOID **) &Usb2HcPpi
                 );
      if (EFI_ERROR (Status)) {
        //
        // No more host controller, break out
        //
        break;
      }
      PeiUsbEnumeration ((EFI_PEI_SERVICES **) PeiServices, NULL, Usb2HcPpi);
      Index++;
    }
  }

  if (Index == 0) {
    return EFI_UNSUPPORTED;
  }

  return EFI_SUCCESS;
}

/**
  The Hub Enumeration just scans the hub ports one time. It also
  doesn't support hot-plug.

  @param  PeiServices            Describes the list of possible PEI Services.
  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.
  @param  CurrentAddress         The DeviceAddress of usb device.

  @retval EFI_SUCCESS            The usb hub is enumerated successfully.
  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
  @retval Others                 Other failure occurs.

**/
EFI_STATUS
PeiHubEnumeration (
  IN EFI_PEI_SERVICES               **PeiServices,
  IN PEI_USB_DEVICE                 *PeiUsbDevice,
  IN UINT8                          *CurrentAddress
  )
{
  UINTN                 Index;
  EFI_STATUS            Status;
  PEI_USB_IO_PPI        *UsbIoPpi;
  EFI_USB_PORT_STATUS   PortStatus;
  UINTN                 MemPages;
  EFI_PHYSICAL_ADDRESS  AllocateAddress;
  PEI_USB_DEVICE        *NewPeiUsbDevice;
  UINTN                 InterfaceIndex;
  UINTN                 EndpointIndex;


  UsbIoPpi    = &PeiUsbDevice->UsbIoPpi;

  DEBUG ((EFI_D_INFO, "PeiHubEnumeration: DownStreamPortNo: %x\n", PeiUsbDevice->DownStreamPortNo));

  for (Index = 0; Index < PeiUsbDevice->DownStreamPortNo; Index++) {

    Status = PeiHubGetPortStatus (
              PeiServices,
              UsbIoPpi,
              (UINT8) (Index + 1),
              (UINT32 *) &PortStatus
              );

    if (EFI_ERROR (Status)) {
      continue;
    }

    DEBUG ((EFI_D_INFO, "USB Status --- Port: %x ConnectChange[%04x] Status[%04x]\n", Index, PortStatus.PortChangeStatus, PortStatus.PortStatus));
    //
    // Only handle connection/enable/overcurrent/reset change.
    //
    if ((PortStatus.PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
      continue;
    } else {
      if (IsPortConnect (PortStatus.PortStatus)) {
        //
        // Begin to deal with the new device
        //
        MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;
        Status = PeiServicesAllocatePages (
                   EfiBootServicesCode,
                   MemPages,
                   &AllocateAddress
                   );
        if (EFI_ERROR (Status)) {
          return EFI_OUT_OF_RESOURCES;
        }

        NewPeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);
        ZeroMem (NewPeiUsbDevice, sizeof (PEI_USB_DEVICE));

        NewPeiUsbDevice->Signature        = PEI_USB_DEVICE_SIGNATURE;
        NewPeiUsbDevice->DeviceAddress    = 0;
        NewPeiUsbDevice->MaxPacketSize0   = 8;
        NewPeiUsbDevice->DataToggle       = 0;
        CopyMem (
          &(NewPeiUsbDevice->UsbIoPpi),
          &mUsbIoPpi,
          sizeof (PEI_USB_IO_PPI)
          );
        CopyMem (
          &(NewPeiUsbDevice->UsbIoPpiList),
          &mUsbIoPpiList,
          sizeof (EFI_PEI_PPI_DESCRIPTOR)
          );
        NewPeiUsbDevice->UsbIoPpiList.Ppi = &NewPeiUsbDevice->UsbIoPpi;
        NewPeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;
        NewPeiUsbDevice->UsbHcPpi         = PeiUsbDevice->UsbHcPpi;
        NewPeiUsbDevice->Usb2HcPpi        = PeiUsbDevice->Usb2HcPpi;
        NewPeiUsbDevice->Tier             = (UINT8) (PeiUsbDevice->Tier + 1);
        NewPeiUsbDevice->IsHub            = 0x0;
        NewPeiUsbDevice->DownStreamPortNo = 0x0;

        if (((PortStatus.PortChangeStatus & USB_PORT_STAT_C_RESET) == 0) ||
             ((PortStatus.PortStatus & (USB_PORT_STAT_CONNECTION | USB_PORT_STAT_ENABLE)) == 0)) {
          //
          // If the port already has reset change flag and is connected and enabled, skip the port reset logic.
          //
          PeiResetHubPort (PeiServices, UsbIoPpi, (UINT8)(Index + 1));

          PeiHubGetPortStatus (
             PeiServices,
             UsbIoPpi,
             (UINT8) (Index + 1),
             (UINT32 *) &PortStatus
             );
        } else {
          PeiHubClearPortFeature (
            PeiServices,
            UsbIoPpi,
            (UINT8) (Index + 1),
            EfiUsbPortResetChange
            );
        }

        NewPeiUsbDevice->DeviceSpeed = (UINT8) PeiUsbGetDeviceSpeed (PortStatus.PortStatus);
        DEBUG ((EFI_D_INFO, "Device Speed =%d\n", PeiUsbDevice->DeviceSpeed));

        if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_SUPER_SPEED)){
          NewPeiUsbDevice->MaxPacketSize0 = 512;
        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_HIGH_SPEED)) {
          NewPeiUsbDevice->MaxPacketSize0 = 64;
        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_LOW_SPEED)) {
          NewPeiUsbDevice->MaxPacketSize0 = 8;
        } else {
          NewPeiUsbDevice->MaxPacketSize0 = 8;
        }

        if(NewPeiUsbDevice->DeviceSpeed != EFI_USB_SPEED_HIGH) {
          if (PeiUsbDevice->DeviceSpeed == EFI_USB_SPEED_HIGH) {
            NewPeiUsbDevice->Translator.TranslatorPortNumber = (UINT8)Index;
            NewPeiUsbDevice->Translator.TranslatorHubAddress = *CurrentAddress;
          } else {
            CopyMem(&(NewPeiUsbDevice->Translator), &(PeiUsbDevice->Translator), sizeof(EFI_USB2_HC_TRANSACTION_TRANSLATOR));
          }
        }

        //
        // Configure that Usb Device
        //
        Status = PeiConfigureUsbDevice (
                  PeiServices,
                  NewPeiUsbDevice,
                  (UINT8) (Index + 1),
                  CurrentAddress
                  );

        if (EFI_ERROR (Status)) {
          continue;
        }
        DEBUG ((EFI_D_INFO, "PeiHubEnumeration: PeiConfigureUsbDevice Success\n"));

        Status = PeiServicesInstallPpi (&NewPeiUsbDevice->UsbIoPpiList);

        if (NewPeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {
          NewPeiUsbDevice->IsHub  = 0x1;

          Status = PeiDoHubConfig (PeiServices, NewPeiUsbDevice);
          if (EFI_ERROR (Status)) {
            return Status;
          }

          PeiHubEnumeration (PeiServices, NewPeiUsbDevice, CurrentAddress);
        }

        for (InterfaceIndex = 1; InterfaceIndex < NewPeiUsbDevice->ConfigDesc->NumInterfaces; InterfaceIndex++) {
          //
          // Begin to deal with the new device
          //
          MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;
          Status = PeiServicesAllocatePages (
                     EfiBootServicesCode,
                     MemPages,
                     &AllocateAddress
                     );
          if (EFI_ERROR (Status)) {
            return EFI_OUT_OF_RESOURCES;
          }
          CopyMem ((VOID *)(UINTN)AllocateAddress, NewPeiUsbDevice, sizeof (PEI_USB_DEVICE));
          NewPeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);
          NewPeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;
          NewPeiUsbDevice->UsbIoPpiList.Ppi = &NewPeiUsbDevice->UsbIoPpi;
          NewPeiUsbDevice->InterfaceDesc = NewPeiUsbDevice->InterfaceDescList[InterfaceIndex];
          for (EndpointIndex = 0; EndpointIndex < NewPeiUsbDevice->InterfaceDesc->NumEndpoints; EndpointIndex++) {
            NewPeiUsbDevice->EndpointDesc[EndpointIndex] = NewPeiUsbDevice->EndpointDescList[InterfaceIndex][EndpointIndex];
          }

          Status = PeiServicesInstallPpi (&NewPeiUsbDevice->UsbIoPpiList);

          if (NewPeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {
            NewPeiUsbDevice->IsHub  = 0x1;

            Status = PeiDoHubConfig (PeiServices, NewPeiUsbDevice);
            if (EFI_ERROR (Status)) {
              return Status;
            }

            PeiHubEnumeration (PeiServices, NewPeiUsbDevice, CurrentAddress);
          }
        }
      }
    }
  }


  return EFI_SUCCESS;
}

/**
  The enumeration routine to detect device change.

  @param  PeiServices            Describes the list of possible PEI Services.
  @param  UsbHcPpi               The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.
  @param  Usb2HcPpi              The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.

  @retval EFI_SUCCESS            The usb is enumerated successfully.
  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
  @retval Others                 Other failure occurs.

**/
EFI_STATUS
PeiUsbEnumeration (
  IN EFI_PEI_SERVICES               **PeiServices,
  IN PEI_USB_HOST_CONTROLLER_PPI    *UsbHcPpi,
  IN PEI_USB2_HOST_CONTROLLER_PPI   *Usb2HcPpi
  )
{
  UINT8                 NumOfRootPort;
  EFI_STATUS            Status;
  UINT8                 Index;
  EFI_USB_PORT_STATUS   PortStatus;
  PEI_USB_DEVICE        *PeiUsbDevice;
  UINTN                 MemPages;
  EFI_PHYSICAL_ADDRESS  AllocateAddress;
  UINT8                 CurrentAddress;
  UINTN                 InterfaceIndex;
  UINTN                 EndpointIndex;

  CurrentAddress = 0;
  if (Usb2HcPpi != NULL) {
    Usb2HcPpi->GetRootHubPortNumber (
                PeiServices,
                Usb2HcPpi,
                (UINT8 *) &NumOfRootPort
                );
  } else if (UsbHcPpi != NULL) {
    UsbHcPpi->GetRootHubPortNumber (
                PeiServices,
                UsbHcPpi,
                (UINT8 *) &NumOfRootPort
                );
  } else {
    ASSERT (FALSE);
    return EFI_INVALID_PARAMETER;
  }

  DEBUG ((EFI_D_INFO, "PeiUsbEnumeration: NumOfRootPort: %x\n", NumOfRootPort));

  for (Index = 0; Index < NumOfRootPort; Index++) {
    //
    // First get root port status to detect changes happen
    //
    if (Usb2HcPpi != NULL) {
      Usb2HcPpi->GetRootHubPortStatus (
                  PeiServices,
                  Usb2HcPpi,
                  (UINT8) Index,
                  &PortStatus
                  );
    } else {
      UsbHcPpi->GetRootHubPortStatus (
                  PeiServices,
                  UsbHcPpi,
                  (UINT8) Index,
                  &PortStatus
                  );
    }
    DEBUG ((EFI_D_INFO, "USB Status --- Port: %x ConnectChange[%04x] Status[%04x]\n", Index, PortStatus.PortChangeStatus, PortStatus.PortStatus));
    //
    // Only handle connection/enable/overcurrent/reset change.
    //
    if ((PortStatus.PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
      continue;
    } else {
      if (IsPortConnect (PortStatus.PortStatus)) {
        MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;
        Status = PeiServicesAllocatePages (
                   EfiBootServicesCode,
                   MemPages,
                   &AllocateAddress
                   );
        if (EFI_ERROR (Status)) {
          return EFI_OUT_OF_RESOURCES;
        }

        PeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);
        ZeroMem (PeiUsbDevice, sizeof (PEI_USB_DEVICE));

        PeiUsbDevice->Signature         = PEI_USB_DEVICE_SIGNATURE;
        PeiUsbDevice->DeviceAddress     = 0;
        PeiUsbDevice->MaxPacketSize0    = 8;
        PeiUsbDevice->DataToggle        = 0;
        CopyMem (
          &(PeiUsbDevice->UsbIoPpi),
          &mUsbIoPpi,
          sizeof (PEI_USB_IO_PPI)
          );
        CopyMem (
          &(PeiUsbDevice->UsbIoPpiList),
          &mUsbIoPpiList,
          sizeof (EFI_PEI_PPI_DESCRIPTOR)
          );
        PeiUsbDevice->UsbIoPpiList.Ppi  = &PeiUsbDevice->UsbIoPpi;
        PeiUsbDevice->AllocateAddress   = (UINTN) AllocateAddress;
        PeiUsbDevice->UsbHcPpi          = UsbHcPpi;
        PeiUsbDevice->Usb2HcPpi         = Usb2HcPpi;
        PeiUsbDevice->IsHub             = 0x0;
        PeiUsbDevice->DownStreamPortNo  = 0x0;

        if (((PortStatus.PortChangeStatus & USB_PORT_STAT_C_RESET) == 0) ||
             ((PortStatus.PortStatus & (USB_PORT_STAT_CONNECTION | USB_PORT_STAT_ENABLE)) == 0)) {
          //
          // If the port already has reset change flag and is connected and enabled, skip the port reset logic.
          //
          ResetRootPort (
            PeiServices,
            PeiUsbDevice->UsbHcPpi,
            PeiUsbDevice->Usb2HcPpi,
            Index,
            0
            );

          if (Usb2HcPpi != NULL) {
            Usb2HcPpi->GetRootHubPortStatus (
                         PeiServices,
                         Usb2HcPpi,
                         (UINT8) Index,
                         &PortStatus
                         );
          } else {
            UsbHcPpi->GetRootHubPortStatus (
                        PeiServices,
                        UsbHcPpi,
                        (UINT8) Index,
                        &PortStatus
                        );
          }
        } else {
          if (Usb2HcPpi != NULL) {
            Usb2HcPpi->ClearRootHubPortFeature (
                        PeiServices,
                        Usb2HcPpi,
                        (UINT8) Index,
                        EfiUsbPortResetChange
                        );
          } else {
            UsbHcPpi->ClearRootHubPortFeature (
                        PeiServices,
                        UsbHcPpi,
                        (UINT8) Index,
                        EfiUsbPortResetChange
                        );
          }
        }

        PeiUsbDevice->DeviceSpeed = (UINT8) PeiUsbGetDeviceSpeed (PortStatus.PortStatus);
        DEBUG ((EFI_D_INFO, "Device Speed =%d\n", PeiUsbDevice->DeviceSpeed));

        if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_SUPER_SPEED)){
          PeiUsbDevice->MaxPacketSize0 = 512;
        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_HIGH_SPEED)) {
          PeiUsbDevice->MaxPacketSize0 = 64;
        } else if (USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_LOW_SPEED)) {
          PeiUsbDevice->MaxPacketSize0 = 8;
        } else {
          PeiUsbDevice->MaxPacketSize0 = 8;
        }

        //
        // Configure that Usb Device
        //
        Status = PeiConfigureUsbDevice (
                  PeiServices,
                  PeiUsbDevice,
                  Index,
                  &CurrentAddress
                  );

        if (EFI_ERROR (Status)) {
          continue;
        }
        DEBUG ((EFI_D_INFO, "PeiUsbEnumeration: PeiConfigureUsbDevice Success\n"));

        Status = PeiServicesInstallPpi (&PeiUsbDevice->UsbIoPpiList);

        if (PeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {
          PeiUsbDevice->IsHub = 0x1;

          Status = PeiDoHubConfig (PeiServices, PeiUsbDevice);
          if (EFI_ERROR (Status)) {
            return Status;
          }

          PeiHubEnumeration (PeiServices, PeiUsbDevice, &CurrentAddress);
        }

        for (InterfaceIndex = 1; InterfaceIndex < PeiUsbDevice->ConfigDesc->NumInterfaces; InterfaceIndex++) {
          //
          // Begin to deal with the new device
          //
          MemPages = sizeof (PEI_USB_DEVICE) / EFI_PAGE_SIZE + 1;
          Status = PeiServicesAllocatePages (
                     EfiBootServicesCode,
                     MemPages,
                     &AllocateAddress
                     );
          if (EFI_ERROR (Status)) {
            return EFI_OUT_OF_RESOURCES;
          }
          CopyMem ((VOID *)(UINTN)AllocateAddress, PeiUsbDevice, sizeof (PEI_USB_DEVICE));
          PeiUsbDevice = (PEI_USB_DEVICE *) ((UINTN) AllocateAddress);
          PeiUsbDevice->AllocateAddress  = (UINTN) AllocateAddress;
          PeiUsbDevice->UsbIoPpiList.Ppi = &PeiUsbDevice->UsbIoPpi;
          PeiUsbDevice->InterfaceDesc = PeiUsbDevice->InterfaceDescList[InterfaceIndex];
          for (EndpointIndex = 0; EndpointIndex < PeiUsbDevice->InterfaceDesc->NumEndpoints; EndpointIndex++) {
            PeiUsbDevice->EndpointDesc[EndpointIndex] = PeiUsbDevice->EndpointDescList[InterfaceIndex][EndpointIndex];
          }

          Status = PeiServicesInstallPpi (&PeiUsbDevice->UsbIoPpiList);

          if (PeiUsbDevice->InterfaceDesc->InterfaceClass == 0x09) {
            PeiUsbDevice->IsHub = 0x1;

            Status = PeiDoHubConfig (PeiServices, PeiUsbDevice);
            if (EFI_ERROR (Status)) {
              return Status;
            }

            PeiHubEnumeration (PeiServices, PeiUsbDevice, &CurrentAddress);
          }
        }
      } else {
        //
        // Disconnect change happen, currently we don't support
        //
      }
    }
  }

  return EFI_SUCCESS;
}

/**
  Configure new detected usb device.

  @param  PeiServices            Describes the list of possible PEI Services.
  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.
  @param  Port                   The port to be configured.
  @param  DeviceAddress          The device address to be configured.

  @retval EFI_SUCCESS            The new detected usb device is configured successfully.
  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
  @retval Others                 Other failure occurs.

**/
EFI_STATUS
PeiConfigureUsbDevice (
  IN EFI_PEI_SERVICES   **PeiServices,
  IN PEI_USB_DEVICE     *PeiUsbDevice,
  IN UINT8              Port,
  IN OUT UINT8          *DeviceAddress
  )
{
  EFI_USB_DEVICE_DESCRIPTOR   DeviceDescriptor;
  EFI_STATUS                  Status;
  PEI_USB_IO_PPI              *UsbIoPpi;
  UINT8                       Retry;

  UsbIoPpi = &PeiUsbDevice->UsbIoPpi;
  Status   = EFI_SUCCESS;
  ZeroMem (&DeviceDescriptor, sizeof (EFI_USB_DEVICE_DESCRIPTOR));
  //
  // Get USB device descriptor
  //

  for (Retry = 0; Retry < 3; Retry ++) {
    Status = PeiUsbGetDescriptor (
               PeiServices,
               UsbIoPpi,
               (USB_DT_DEVICE << 8),
               0,
               8,
               &DeviceDescriptor
               );

    if (!EFI_ERROR (Status)) {
      DEBUG ((EFI_D_INFO, "PeiUsbGet Device Descriptor the %d time Success\n", Retry));
      break;
    }
  }

  if (Retry == 3) {
    DEBUG ((EFI_D_ERROR, "PeiUsbGet Device Descriptor fail: %x %r\n", Retry, Status));
    return Status;
  }

  if ((DeviceDescriptor.BcdUSB >= 0x0300) && (DeviceDescriptor.MaxPacketSize0 == 9)) {
    PeiUsbDevice->MaxPacketSize0 = 1 << 9;
  } else {
    PeiUsbDevice->MaxPacketSize0 = DeviceDescriptor.MaxPacketSize0;
  }

  (*DeviceAddress) ++;

  Status = PeiUsbSetDeviceAddress (
            PeiServices,
            UsbIoPpi,
            *DeviceAddress
            );

  if (EFI_ERROR (Status)) {
    DEBUG ((EFI_D_ERROR, "PeiUsbSetDeviceAddress Failed: %r\n", Status));
    return Status;
  }
  MicroSecondDelay (USB_SET_DEVICE_ADDRESS_STALL);

  PeiUsbDevice->DeviceAddress = *DeviceAddress;

  //
  // Get whole USB device descriptor
  //
  Status = PeiUsbGetDescriptor (
            PeiServices,
            UsbIoPpi,
            (USB_DT_DEVICE << 8),
            0,
            (UINT16) sizeof (EFI_USB_DEVICE_DESCRIPTOR),
            &DeviceDescriptor
            );

  if (EFI_ERROR (Status)) {
    DEBUG ((EFI_D_ERROR, "PeiUsbGetDescriptor First Failed\n"));
    return Status;
  }

  //
  // Get its default configuration and its first interface
  //
  Status = PeiUsbGetAllConfiguration (
            PeiServices,
            PeiUsbDevice
            );
  if (EFI_ERROR (Status)) {
    return Status;
  }
  MicroSecondDelay (USB_GET_CONFIG_DESCRIPTOR_STALL);

  Status = PeiUsbSetConfiguration (
            PeiServices,
            UsbIoPpi
            );

  if (EFI_ERROR (Status)) {
    return Status;
  }

  return EFI_SUCCESS;
}

/**
  Get all configurations from a detected usb device.

  @param  PeiServices            Describes the list of possible PEI Services.
  @param  PeiUsbDevice           The pointer of PEI_USB_DEVICE instance.

  @retval EFI_SUCCESS            The new detected usb device is configured successfully.
  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
  @retval Others                 Other failure occurs.

**/
EFI_STATUS
PeiUsbGetAllConfiguration (
  IN EFI_PEI_SERVICES   **PeiServices,
  IN PEI_USB_DEVICE     *PeiUsbDevice
  )
{
  EFI_STATUS                Status;
  EFI_USB_CONFIG_DESCRIPTOR *ConfigDesc;
  PEI_USB_IO_PPI            *UsbIoPpi;
  UINT16                    ConfigDescLength;
  UINT8                     *Ptr;
  UINTN                     SkipBytes;
  UINTN                     LengthLeft;
  UINTN                     InterfaceIndex;
  UINTN                     Index;
  UINTN                     NumOfEndpoint;

  UsbIoPpi = &PeiUsbDevice->UsbIoPpi;

  //
  // First get its 4-byte configuration descriptor
  //
  Status = PeiUsbGetDescriptor (
            PeiServices,
            UsbIoPpi,
            (USB_DT_CONFIG << 8), // Value
            0,      // Index
            4,      // Length
            PeiUsbDevice->ConfigurationData
            );

  if (EFI_ERROR (Status)) {
    DEBUG ((EFI_D_ERROR, "PeiUsbGet Config Descriptor First Failed\n"));
    return Status;
  }
  MicroSecondDelay (USB_GET_CONFIG_DESCRIPTOR_STALL);

  ConfigDesc        = (EFI_USB_CONFIG_DESCRIPTOR *) PeiUsbDevice->ConfigurationData;
  ConfigDescLength  = ConfigDesc->TotalLength;

  //
  // Reject if TotalLength even cannot cover itself.
  //
  if (ConfigDescLength < OFFSET_OF (EFI_USB_CONFIG_DESCRIPTOR, TotalLength) + sizeof (ConfigDesc->TotalLength)) {
    return EFI_DEVICE_ERROR;
  }

  //
  // Reject if TotalLength exceeds the PeiUsbDevice->ConfigurationData.
  //
  if (ConfigDescLength > sizeof (PeiUsbDevice->ConfigurationData)) {
    return EFI_DEVICE_ERROR;
  }

  //
  // Then we get the total descriptors for this configuration
  //
  Status = PeiUsbGetDescriptor (
            PeiServices,
            UsbIoPpi,
            (USB_DT_CONFIG << 8),
            0,
            ConfigDescLength,
            PeiUsbDevice->ConfigurationData
            );

  if (EFI_ERROR (Status)) {
    DEBUG ((EFI_D_ERROR, "PeiUsbGet Config Descriptor all Failed\n"));
    return Status;
  }
  //
  // Parse this configuration descriptor
  // First get the current config descriptor;
  //
  Status = GetExpectedDescriptor (
            PeiUsbDevice->ConfigurationData,
            ConfigDescLength,
            USB_DT_CONFIG,
            (UINT8) sizeof (EFI_USB_CONFIG_DESCRIPTOR),
            &SkipBytes
            );

  if (EFI_ERROR (Status)) {
    return Status;
  }

  Ptr                       = PeiUsbDevice->ConfigurationData + SkipBytes;
  PeiUsbDevice->ConfigDesc  = (EFI_USB_CONFIG_DESCRIPTOR *) Ptr;

  Ptr += sizeof (EFI_USB_CONFIG_DESCRIPTOR);
  LengthLeft = ConfigDescLength - SkipBytes - sizeof (EFI_USB_CONFIG_DESCRIPTOR);

  for (InterfaceIndex = 0; InterfaceIndex < PeiUsbDevice->ConfigDesc->NumInterfaces; InterfaceIndex++) {

    //
    // Get the interface descriptor
    //
    Status = GetExpectedDescriptor (
              Ptr,
              LengthLeft,
              USB_DT_INTERFACE,
              (UINT8) sizeof (EFI_USB_INTERFACE_DESCRIPTOR),
              &SkipBytes
              );

    if (EFI_ERROR (Status)) {
      return Status;
    }

    Ptr += SkipBytes;
    if (InterfaceIndex == 0) {
      PeiUsbDevice->InterfaceDesc = (EFI_USB_INTERFACE_DESCRIPTOR *) Ptr;
    }
    PeiUsbDevice->InterfaceDescList[InterfaceIndex] = (EFI_USB_INTERFACE_DESCRIPTOR *) Ptr;

    Ptr += sizeof (EFI_USB_INTERFACE_DESCRIPTOR);
    LengthLeft -= SkipBytes;
    LengthLeft -= sizeof (EFI_USB_INTERFACE_DESCRIPTOR);

    //
    // Parse all the endpoint descriptor within this interface
    //
    NumOfEndpoint = PeiUsbDevice->InterfaceDescList[InterfaceIndex]->NumEndpoints;
    ASSERT (NumOfEndpoint <= MAX_ENDPOINT);

    for (Index = 0; Index < NumOfEndpoint; Index++) {
      //
      // Get the endpoint descriptor
      //
      Status = GetExpectedDescriptor (
                Ptr,
                LengthLeft,
                USB_DT_ENDPOINT,
                (UINT8) sizeof (EFI_USB_ENDPOINT_DESCRIPTOR),
                &SkipBytes
                );

      if (EFI_ERROR (Status)) {
        return Status;
      }

      Ptr += SkipBytes;
      if (InterfaceIndex == 0) {
        PeiUsbDevice->EndpointDesc[Index] = (EFI_USB_ENDPOINT_DESCRIPTOR *) Ptr;
      }
      PeiUsbDevice->EndpointDescList[InterfaceIndex][Index] = (EFI_USB_ENDPOINT_DESCRIPTOR *) Ptr;

      Ptr += sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);
      LengthLeft -= SkipBytes;
      LengthLeft -= sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);
    }
  }

  return EFI_SUCCESS;
}

/**
  Get the start position of next wanted descriptor.

  @param  Buffer            Buffer containing data to parse.
  @param  Length            Buffer length.
  @param  DescType          Descriptor type.
  @param  DescLength        Descriptor length.
  @param  ParsedBytes       Bytes has been parsed.

  @retval EFI_SUCCESS       Get wanted descriptor successfully.
  @retval EFI_DEVICE_ERROR  Error occurred.

**/
EFI_STATUS
GetExpectedDescriptor (
  IN  UINT8       *Buffer,
  IN  UINTN       Length,
  IN  UINT8       DescType,
  IN  UINT8       DescLength,
  OUT UINTN       *ParsedBytes
  )
{
  USB_DESC_HEAD   *Head;
  UINTN           Offset;

  //
  // Total length is too small that cannot hold the single descriptor header plus data. 
  //
  if (Length <= sizeof (USB_DESC_HEAD)) {
    DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: met mal-format descriptor, total length = %d!\n", Length));
    return EFI_DEVICE_ERROR;
  }

  //
  // All the descriptor has a common LTV (Length, Type, Value)
  // format. Skip the descriptor that isn't of this Type
  //
  Offset = 0;
  Head   = (USB_DESC_HEAD *)Buffer;
  while (Offset < Length - sizeof (USB_DESC_HEAD)) {
    //
    // Above condition make sure Head->Len and Head->Type are safe to access
    //
    Head = (USB_DESC_HEAD *)&Buffer[Offset];

    if (Head->Len == 0) {
      DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: met mal-format descriptor, Head->Len = 0!\n"));
      return EFI_DEVICE_ERROR;
    }

    //
    // Make sure no overflow when adding Head->Len to Offset.
    //
    if (Head->Len > MAX_UINTN - Offset) {
      DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: met mal-format descriptor, Head->Len = %d!\n", Head->Len));
      return EFI_DEVICE_ERROR;
    }

    if (Head->Type == DescType) {
      break;
    }

    Offset += Head->Len;
  }

  //
  // Head->Len is invalid resulting data beyond boundary, or
  // Descriptor cannot be found: No such type.
  //
  if (Length < Offset) {
    DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: met mal-format descriptor, Offset/Len = %d/%d!\n", Offset, Length));
    return EFI_DEVICE_ERROR;
  }

  if ((Head->Type != DescType) || (Head->Len < DescLength)) {
    DEBUG ((DEBUG_ERROR, "GetExpectedDescriptor: descriptor cannot be found, Header(T/L) = %d/%d!\n", Head->Type, Head->Len));
    return EFI_DEVICE_ERROR;
  }

  *ParsedBytes = Offset;
  return EFI_SUCCESS;
}

/**
  Send reset signal over the given root hub port.

  @param  PeiServices       Describes the list of possible PEI Services.
  @param  UsbHcPpi          The pointer of PEI_USB_HOST_CONTROLLER_PPI instance.
  @param  Usb2HcPpi         The pointer of PEI_USB2_HOST_CONTROLLER_PPI instance.
  @param  PortNum           The port to be reset.
  @param  RetryIndex        The retry times.

**/
VOID
ResetRootPort (
  IN EFI_PEI_SERVICES               **PeiServices,
  IN PEI_USB_HOST_CONTROLLER_PPI    *UsbHcPpi,
  IN PEI_USB2_HOST_CONTROLLER_PPI   *Usb2HcPpi,
  IN UINT8                          PortNum,
  IN UINT8                          RetryIndex
  )
{
  EFI_STATUS             Status;
  UINTN                  Index;
  EFI_USB_PORT_STATUS    PortStatus;


  if (Usb2HcPpi != NULL) {
    MicroSecondDelay (200 * 1000);

    //
    // reset root port
    //
    Status = Usb2HcPpi->SetRootHubPortFeature (
                         PeiServices,
                         Usb2HcPpi,
                         PortNum,
                         EfiUsbPortReset
                         );

    if (EFI_ERROR (Status)) {
      DEBUG ((EFI_D_ERROR, "SetRootHubPortFeature EfiUsbPortReset Failed\n"));
      return;
    }

    //
    // Drive the reset signal for at least 50ms. Check USB 2.0 Spec
    // section 7.1.7.5 for timing requirements.
    //
    MicroSecondDelay (USB_SET_ROOT_PORT_RESET_STALL);

    //
    // clear reset root port
    //
    Status = Usb2HcPpi->ClearRootHubPortFeature (
                         PeiServices,
                         Usb2HcPpi,
                         PortNum,
                         EfiUsbPortReset
                         );

    if (EFI_ERROR (Status)) {
      DEBUG ((EFI_D_ERROR, "ClearRootHubPortFeature EfiUsbPortReset Failed\n"));
      return;
    }

    MicroSecondDelay (USB_CLR_ROOT_PORT_RESET_STALL);

    //
    // USB host controller won't clear the RESET bit until
    // reset is actually finished.
    //
    ZeroMem (&PortStatus, sizeof (EFI_USB_PORT_STATUS));

    for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
      Status = Usb2HcPpi->GetRootHubPortStatus (
                            PeiServices,
                            Usb2HcPpi,
                            PortNum,
                            &PortStatus
                            );
      if (EFI_ERROR (Status)) {
        return;
      }

      if (!USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_RESET)) {
        break;
      }

      MicroSecondDelay (USB_WAIT_PORT_STS_CHANGE_STALL);
    }

    if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {
      DEBUG ((EFI_D_ERROR, "ResetRootPort: reset not finished in time on port %d\n", PortNum));
      return;
    }

    Usb2HcPpi->ClearRootHubPortFeature (
                PeiServices,
                Usb2HcPpi,
                PortNum,
                EfiUsbPortResetChange
                );

    Usb2HcPpi->ClearRootHubPortFeature (
                PeiServices,
                Usb2HcPpi,
                PortNum,
                EfiUsbPortConnectChange
                );

    //
    // Set port enable
    //
    Usb2HcPpi->SetRootHubPortFeature(
                PeiServices,
                Usb2HcPpi,
                PortNum,
                EfiUsbPortEnable
                );

    Usb2HcPpi->ClearRootHubPortFeature (
                PeiServices,
                Usb2HcPpi,
                PortNum,
                EfiUsbPortEnableChange
                );

    MicroSecondDelay ((RetryIndex + 1) * 50 * 1000);
  } else {
    MicroSecondDelay (200 * 1000);

    //
    // reset root port
    //
    Status = UsbHcPpi->SetRootHubPortFeature (
                         PeiServices,
                         UsbHcPpi,
                         PortNum,
                         EfiUsbPortReset
                         );

    if (EFI_ERROR (Status)) {
      DEBUG ((EFI_D_ERROR, "SetRootHubPortFeature EfiUsbPortReset Failed\n"));
      return;
    }

    //
    // Drive the reset signal for at least 50ms. Check USB 2.0 Spec
    // section 7.1.7.5 for timing requirements.
    //
    MicroSecondDelay (USB_SET_ROOT_PORT_RESET_STALL);

    //
    // clear reset root port
    //
    Status = UsbHcPpi->ClearRootHubPortFeature (
                         PeiServices,
                         UsbHcPpi,
                         PortNum,
                         EfiUsbPortReset
                         );

    if (EFI_ERROR (Status)) {
      DEBUG ((EFI_D_ERROR, "ClearRootHubPortFeature EfiUsbPortReset Failed\n"));
      return;
    }

    MicroSecondDelay (USB_CLR_ROOT_PORT_RESET_STALL);

    //
    // USB host controller won't clear the RESET bit until
    // reset is actually finished.
    //
    ZeroMem (&PortStatus, sizeof (EFI_USB_PORT_STATUS));

    for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
      Status = UsbHcPpi->GetRootHubPortStatus (
                           PeiServices,
                           UsbHcPpi,
                           PortNum,
                           &PortStatus
                           );
      if (EFI_ERROR (Status)) {
        return;
      }

      if (!USB_BIT_IS_SET (PortStatus.PortStatus, USB_PORT_STAT_RESET)) {
        break;
      }

      MicroSecondDelay (USB_WAIT_PORT_STS_CHANGE_STALL);
    }

    if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {
      DEBUG ((EFI_D_ERROR, "ResetRootPort: reset not finished in time on port %d\n", PortNum));
      return;
    }

    UsbHcPpi->ClearRootHubPortFeature (
                PeiServices,
                UsbHcPpi,
                PortNum,
                EfiUsbPortResetChange
                );

    UsbHcPpi->ClearRootHubPortFeature (
                PeiServices,
                UsbHcPpi,
                PortNum,
                EfiUsbPortConnectChange
                );

    //
    // Set port enable
    //
    UsbHcPpi->SetRootHubPortFeature(
                PeiServices,
                UsbHcPpi,
                PortNum,
                EfiUsbPortEnable
                );

    UsbHcPpi->ClearRootHubPortFeature (
                PeiServices,
                UsbHcPpi,
                PortNum,
                EfiUsbPortEnableChange
                );

    MicroSecondDelay ((RetryIndex + 1) * 50 * 1000);
  }
  return;
}